JP2005044230A - Robot control device - Google Patents

Abstract

PROBLEM TO BE SOLVED: To increase the speed of operation by making it possible to determine the maximum speed upper limit value of linear interpolation or circular interpolation for each operation in consideration of the allowable value of a motor or a transmission mechanism.
SOLUTION: A reference speed calculation means 2 calculates a reference speed in each direction in each axis of the current operation or in an orthogonal coordinate system, and an acceleration / deceleration time / speed upper limit parameter determination means 7 is orthogonal to the acceleration start point and the deceleration end point. In response to coordinate values and joint displacements, the shortest acceleration within a range that satisfies the constraints of the maximum allowable torque of the motor that drives each axis for each operation, the maximum operating torque and the maximum allowable moment of the transmission mechanism of each axis Time, deceleration time, and maximum speed upper limit parameter are calculated, and a command curve is generated based on the calculated data.
[Selection] Figure 1

Description

  The present invention relates to a robot control device that controls the operating speed of a motor that drives each axis of a robot.

When the robot is operated, even if the acceleration is the same, the required torque changes according to the posture of the robot. Therefore, a method of optimizing parameters such as acceleration / deceleration time is taken for each operation. A conventional robot control apparatus to which this method is applied (for example, see Patent Document 1) operates in the following configuration.
Provided with teaching point storage means for storing the coordinates of the robot motion start point (acceleration start point) and motion end point (deceleration end point) as teaching points, and the maximum speed parameter value for each axis or direction, etc. Parameter storage means stored in advance is provided. When the movement command is received by the teaching point storage means, the movement command is analyzed to determine from which point to which point the robot is moved. For example, when the movement command is a linear interpolation command from point A (acceleration start point) to point B (deceleration end point), the teaching point storage means stores the orthogonal coordinate values and joint displacement values of points A and B. Output data. Next, the maximum speed calculation means calculates the maximum speed in each direction in the operation based on the orthogonal coordinate value and the maximum speed parameter value of the parameter storage means. Next, based on the orthogonal coordinate value and joint displacement output from the teaching point storage means, parameters such as torque or force tolerance stored in the parameter storage means, and the maximum speed calculated by the maximum speed calculation means The acceleration / deceleration time determining means determines the optimum acceleration / deceleration time for the operation. When the maximum speed and the acceleration / deceleration time are calculated, a speed command curve is generated by the command curve generating means based on these values. In this case, a position command is generated so that the speed command curve becomes a speed trapezoid command curve, and smoothing processing is performed on the position command using a moving average filter, a first-order lag filter, or the like. As a result, the motor control means controls the drive motor of each axis so that the robot follows the generated command curve.

Here, the acceleration / deceleration time determination operation performed by the acceleration / deceleration time determination means is performed as follows.
First, initial values of acceleration time and deceleration time are calculated. Next, the position and speed at the acceleration end point and the deceleration start point are obtained, and the acceleration time is recalculated at the acceleration start point and the acceleration end point. Then, the acceleration time calculated at the acceleration start point and the acceleration time calculated at the acceleration end point are compared, and the larger one is set as the acceleration time. Similarly, the deceleration time is recalculated at the deceleration start point and the deceleration end point. Then, the deceleration time calculated at the deceleration start point and the deceleration time calculated at the deceleration end point are compared, and the larger one is set as the deceleration time. When the number of repetitions of the series of processes reaches the designated number, the current calculation result is output as the acceleration time and the deceleration time. If the number of repetitions has not reached the specified number of times, the above series of processing is repeated, and the position and speed of the acceleration end point and deceleration start point are determined based on the current acceleration time and deceleration time, and the acceleration / deceleration time is calculated again. I do. The acceleration time is calculated as follows at the acceleration start point and the acceleration end point.

The equation of motion of the robot is that the driving torque vector composed of the driving torque of each joint is τ, the inertia matrix is M, the vector composed of the sum of centrifugal / Coriolis force, gravity and frictional force of each joint is h, If a vector composed of the acceleration of
τ = Ma + h (1)
It becomes. Here, when the speed command in each direction is a trapezoidal speed command, if the vector composed of the respective axis speeds at the acceleration end point is v _k and the acceleration time is t _k , the equation of motion of the robot in the acceleration section is
τ = Mv _k / t _k + h (2)
It becomes. Acceleration time t that is the shortest in the range where the result of calculating M and h in Expression (2) based on the position and speed of the acceleration start point satisfies the constraint that the value is equal to or less than the allowable maximum torque of each axis. _{As long as k1} is calculated, and M and h in the formula (2) are calculated based on the position and speed of the acceleration end point, the range satisfying the constraint that any axis is less than or equal to the allowable maximum torque of each axis. The shortest acceleration time t _k2 is calculated. The calculated t _k1 and t _k2 are compared, and the larger one is used as the acceleration time t _k . On the other hand, the equation of motion of the deceleration section, the deceleration time t _g, the vector composed of the shaft speed of the deceleration start point and v _g,
τ = −Mv _g / t _g + h (3)
Therefore, based on the expression (3), the deceleration time is calculated from the position and speed of the deceleration start point and the deceleration end point by the same method as the calculation of the acceleration time.

Japanese Patent Laid-Open No. 7-200033

  Since the conventional robot control device is configured as described above, when performing linear interpolation that moves the robot's hand along a straight line or circular interpolation that moves along a circular arc, even in a constant velocity section, the joint Since the acceleration / deceleration is performed at the level, if the speed in the constant speed section is too high, the motor or transmission mechanism may be exceeded only by continuing constant speed operation. An upper limit could not be established. In addition, when performing continuous operation, the temperature rise amount may increase only for the motor of a specific axis. To prevent this, the acceleration correction ratio for each axis, the speed correction ratio for each axis, and each axis There is a problem that the operation parameters such as the acceleration time and the deceleration time cannot be automatically corrected according to the ratio specified in advance such as the waiting time correction ratio and the motor load of each axis.

  The present invention has been made to solve the above-described problems. A first object of the present invention is to obtain a robot control apparatus capable of increasing the operation speed by a novel method.

  The robot control device according to the present invention includes an allowable maximum speed of each axis of the robot, a reference maximum speed in an orthogonal coordinate system, an allowable maximum value of a driving torque of a motor that drives each axis, an operating torque and a moment of a transmission mechanism of each axis. Is stored in advance in the robot parameter storage means, and the orthogonal coordinate values and joint displacements of the acceleration start point and deceleration end point input when moving by linear interpolation by the reference speed calculation means, and each axis Based on the maximum allowable speed or the reference maximum speed in the Cartesian coordinate system, calculate the reference speed that is reached when the acceleration time has elapsed since the start of the movement in each axis or the Cartesian coordinate system for the current motion of the robot. By the time / speed upper limit parameter determination means, at least one of the Cartesian coordinate value of the input acceleration start point and deceleration end point and the joint displacement In response to the above, at least one of the allowable maximum value of the driving torque of the motor that drives each axis for each operation, the allowable maximum value of the operating torque and moment of the transmission mechanism of each axis, and the allowable maximum speed of each axis is set. Calculate the shortest acceleration time, deceleration time, and maximum speed upper limit parameter within the range to be satisfied, and based on the calculated reference speed, acceleration time, deceleration time, and maximum speed upper limit parameter of the current operation, A command curve for controlling a motor for driving each axis of the robot is generated.

  The robot control device according to the present invention includes an allowable maximum speed of each axis of the robot, a reference maximum speed in an orthogonal coordinate system, an allowable maximum value of a driving torque of a motor that drives each axis, an operating torque and a moment of a transmission mechanism of each axis. Is stored in advance in the robot parameter storage means, and the reference speed calculation means decelerates from the acceleration start point in response to the orthogonal coordinate values and joint displacements of the input acceleration start point and deceleration end point. When the movement to the end point is joint interpolation, the reference speed reached when the acceleration time elapses from the start of each axis of the current movement is calculated based on the maximum allowable speed of each axis of the robot, and the acceleration starts When the movement from the point to the deceleration end point is linear interpolation, the acceleration time elapses from the start of the operation in each direction of the orthogonal coordinate system of the current operation based on the reference maximum speed in the orthogonal coordinate system. The reference speed that is reached is calculated, and each axis is driven for each operation in response to at least one of the orthogonal coordinate value and the joint displacement of the input acceleration start point and deceleration end point by the acceleration / deceleration time determining means. Calculate the shortest acceleration time and deceleration time within a range that satisfies at least one of the allowable maximum value of the motor driving torque, the operating torque of the transmission mechanism of each axis, and the allowable maximum value of the moment. Calculate the torque usage rate of the acceleration section of each axis based on the calculated shortest acceleration time, calculate the torque usage rate of the deceleration section of each axis based on the shortest deceleration time, The correction rate is stored in advance in the correction rate storage means, and is calculated based on the torque usage rate and the correction rate for each axis calculated by the operation parameter correction means in the acceleration and deceleration sections of each axis. The operation parameters are corrected, and based on the calculated reference speed, acceleration time, deceleration time and the corrected operation parameters, a command curve for controlling the motor that drives each axis of the robot is generated by the command curve generation means. It is a thing.

  According to the present invention, since the maximum speed upper limit value and acceleration / deceleration parameters can be set within a range that satisfies the restrictions on the allowable torque of the motor and the transmission mechanism for each operation, there is an effect that the operation speed of the robot can be increased.

  According to the present invention, since the operation parameter can be corrected according to the ratio specified in advance and the load of the motor of each axis, there is an effect that the robot can be speeded up while suppressing the load of the specified motor.

Embodiment 1 FIG.
FIG. 1 is a block diagram showing a configuration of a robot control apparatus according to Embodiment 1 of the present invention. In the figure, the teaching point storage means 1 stores the orthogonal coordinates of the acceleration start point and deceleration end point of the robot. For example, when a movement command for moving from point A to point B is received, the teaching point storage unit 1 It is means for outputting orthogonal coordinate values and joint displacement. The robot parameter storage means 6 includes an allowable maximum speed of each axis determined for each robot model, a reference maximum speed in an orthogonal coordinate system, an allowable maximum value of a driving torque of a motor that drives each axis, and a transmission mechanism of each axis. , And the parameter values for expressing the dynamic conditions of the robot such as the mass and the position of the center of gravity in advance. The reference speed calculation means 2 is based on the maximum allowable speed for each axis of the robot stored in the robot parameter storage means 6 or the reference maximum speed in the Cartesian coordinate system. Is a means for calculating a reference speed that is reached when an acceleration time has elapsed since the start of operation in each direction.

  The acceleration / deceleration time / speed upper limit parameter determination means 7 is a restriction on the allowable maximum value of the motor driving torque, the operating torque of the transmission mechanism of each axis, and the allowable maximum value of moment stored in the robot parameter storage means 6 for each operation. This is means for calculating the shortest acceleration time, the shortest deceleration time, and the maximum speed upper limit parameter within a range satisfying the above. The command curve generation means 3 is a reference speed of the current motion of the robot calculated by the reference speed calculation means 2, an acceleration time, a deceleration time and a speed upper limit parameter of the motion calculated by the acceleration / deceleration time / speed upper limit parameter determination means 7. Is a means for generating a command curve based on The motor control unit 4 controls a motor that drives each axis of the robot 5 according to the command curve generated by the command curve generation unit 3.

Next, the operation will be described.
When the teaching point storage means 1 receives, for example, a command to move by linear interpolation from point A (acceleration start point) to point B (deceleration end point), the orthogonal coordinates of the points A and B are received from the teaching point storage means 1. The value and the joint displacement are read and provided to the reference speed calculation means 2 and the acceleration / deceleration time / speed upper limit parameter determination means 7. Based on these data and the reference maximum speed in each direction of the orthogonal coordinate system stored in the robot parameter storage means 6, the reference speed calculation means 2 calculates the reference speed in each direction in the operation, The speed upper limit parameter determining means 7 and the command curve generating means 3 are given.

In the acceleration / deceleration time / speed upper limit parameter determination means 7, the constraints of the allowable maximum value of the motor driving torque, the operating torque of the transmission mechanism of each axis, and the allowable maximum value of the moment are satisfied according to the processing procedure shown in FIG. The shortest acceleration time, deceleration time and maximum speed upper limit parameter in the range are determined.
First, initial values of the acceleration time and deceleration time of the operation, and a magnification allowable value to be multiplied by the reference speed, that is, an initial value of the speed upper limit parameter are set (step ST11). Next, the number of repetitions n is set to an initial value 1 (step ST12). Orthogonal coordinate values of points A and B given from the teaching point storage means 1, the reference speed in each direction in the operation calculated by the reference speed calculation means 2, the acceleration / deceleration time and speed upper limit parameters in the current operation, Then, the Cartesian coordinate value, the joint displacement, the speed in each direction, and the speed of each axis at the acceleration end point and the deceleration start point are calculated (step ST13).

Next, the shortest acceleration time t _1ai (i = 1 to j, j is the number of axes of the robot) is calculated within the range satisfying the allowable torque constraints of the motor and the transmission mechanism at the acceleration start point, and the obtained t _{1ai. Is} the maximum value of t _1a . Similarly, the shortest acceleration time t _1bi is calculated within a range that satisfies the restrictions on the allowable torque of the motor and the transmission mechanism at the acceleration end point, and the maximum value of t _1bi is set to t _1b (step ST14). Among these calculated maximum values t _1a and t _1b , the larger value is determined as the current acceleration time t ₁ (step ST15).
Similarly, the deceleration time is obtained. First, the shortest deceleration time t _2ai (i = 1 to j, j is the number of axes of the robot) is calculated within the range satisfying the allowable torque constraint of the motor and the transmission mechanism at the deceleration start point, and the obtained t _2ai Let the maximum value be _t2a . Similarly, the shortest deceleration time t _2bi is calculated within a range that satisfies the restrictions on the allowable torque of the motor and the transmission mechanism at the deceleration end point, and the obtained maximum value of t _2bi is set to t _2b (step ST16). Among these calculated maximum values t _2a and t _2b , the larger value is determined as the current deceleration time t ₂ (step ST17).

Next, the joint displacement of the acceleration end point, from the direction of the velocity, and calculates the maximum speed limit parameter v _a can continue the constant velocity movement range satisfying the constraints of the allowable torque of each axis motor and the transmission mechanism . Further, the maximum speed upper limit parameter v _b that allows the constant speed motion to be continued within the range satisfying the restriction of the allowable torque of the motor and transmission mechanism of each axis is calculated from the joint displacement at the deceleration start point and the speed in each direction ( Step ST18). The current speed upper limit parameter vmax (n−1) and the smallest calculated maximum speed upper limit parameter v _a , v _b are determined as the speed upper limit parameter vmax (n) in the current iteration (step ST19). .

  If the number of repetitions n exceeds the specified value K, the acceleration time kt finally obtained by the acceleration / deceleration time / speed upper limit parameter determination means 7 is finally obtained for the current acceleration time, deceleration time, and speed upper limit parameter. The deceleration time gt and the finally obtained speed upper limit parameter vmax are output to the command curve generating means 3 (step ST20). If the number of repetitions n does not exceed the specified value K, the number of repetitions n is increased by 1 (step ST21), and the calculation from step ST13 to step ST19 is performed again to calculate the acceleration time, deceleration time, and speed upper limit parameter. Do.

  In the command curve generating means 3, the command is generated every moment based on the reference speed output from the reference speed calculating means 2 and the acceleration time, deceleration time and speed upper limit parameters output from the acceleration / deceleration time / speed upper limit parameter determining means 7. A curve is generated and output to the motor control means 4. The motor control unit 4 controls the motor that drives the robot 5 according to the command curve generated by the command curve generation unit 3 every moment.

Next, details of calculation of speed and displacement at the acceleration end point and deceleration start point in step ST13, calculation of acceleration time and deceleration time in steps ST14 and ST16, and calculation of a speed upper limit parameter in step ST18 will be described.
First, in step ST13, the linear speed v _l of this operation is calculated from the reference speed v ₀ and the speed upper limit parameter vmax by the equation (4).
v _l = vmax ^* v ₀ (4)
Next, from the linear velocity v _l , the orthogonal coordinate value of the operation start point and the orthogonal coordinate value of the operation end point, the current acceleration time, and the deceleration time, the acceleration end point (point C) and the deceleration start point (point D) An orthogonal coordinate value is calculated. From the calculated orthogonal coordinate values and joint displacement q _c of the acceleration end point is calculated joint displacement q _d of the deceleration start point. The Cartesian coordinate value and the joint displacement q _c2 of the point (point C2) advanced from the acceleration end point to the operation end point B at the linear velocity v ₁ for a minute time Δt are calculated. Further, an orthogonal coordinate value and a joint displacement q _d2 at a point (point D2) that returns from the deceleration start point to the operation start point A at a linear speed v ₁ for a minute time Δt are calculated.

Furthermore, the joint displacement at the acceleration start point (point A) is q _a , the joint displacement at the point (A2) point advanced from the acceleration start point by the linear speed v ₁ for a minute time Δt, q _a2 , and the deceleration end point (point B) ) Is the joint displacement q _b , and the joint displacement at the point (B2 point) where the linear velocity v ₁ returns to the operation start point A from the deceleration end point for a minute time Δt is q _b2 . At this time, the joint velocity vectors v _1a , v _1b , v _2a , and v _2b at the acceleration start point, the acceleration end point, the deceleration start point, and the deceleration end point are respectively calculated by Equations (5) to (8).
v _1a = (q _a2 −q _a ) / Δt (5)
v _1b = (q _c2 −q _c ) / Δt (6)
v _2a = (q _d −q _d2 ) / Δt (7)
v _2b = (q _b −q _b2 ) / Δt (8)

In step ST14, the acceleration time is calculated from the acceleration start point and the acceleration end point. First, let M _{l be} a matrix obtained by removing the inertia of the motor from the inertia matrix of the motion equation of the robot, and let h _l be a vector composed of the sum of centrifugal / Coriolis force and gravity. At this time, the torque τ ₁ acting on the speed reducer of each joint is expressed by Equation (9).
τ _l = M _l a + h _l (9)
The acceleration time is calculated at the acceleration start point and the acceleration end point so that the torque of the motor and the speed reducer calculated by the equations (1) and (9) is less than the allowable value. When accelerating to the speed v with the acceleration time tk, a = v / tk in equations (1) and (9). At the acceleration start point, when the joint displacement is q _a , the current speed is 0, and acceleration is performed at a constant acceleration up to the speed v _1a , the calculation results of formulas (1) and (9) are the motors for both axes. And the acceleration time t _1a that is the shortest within a range satisfying the constraint that is less than or equal to the allowable torque of the reduction gear is calculated. At the acceleration end point, when the joint displacement is q _c , the current speed is v _1b, and acceleration is performed at a constant acceleration up to the speed v _1b , the calculation results of equations (1) and (9) are the same for any axis. An acceleration time t _1b that is the shortest in a range that satisfies a constraint that is less than or equal to the allowable torque of the motor and the reduction gear is calculated.

In step ST16, the deceleration time is calculated at the deceleration start point and the deceleration end point. When decelerating from the speed v with the deceleration time tg, a = −v / tg in equations (1) and (9). At the deceleration start point, when the joint displacement is q _d , the current speed is v _2a, and deceleration is performed at a constant acceleration from the speed v _2a , the calculation results of equations (1) and (9) are the same for any axis. A deceleration time _t2a that is the shortest in a range that satisfies a constraint that is equal to or less than the allowable torque of the motor and the reduction gear is calculated. At the deceleration end point, when the joint displacement is q _b , the current speed is 0, and deceleration is performed at a constant acceleration from the speed v _2b , the calculation results of formulas (1) and (9) are the motors for both axes. And the deceleration time _t2b that is the shortest within the range satisfying the constraint that is less than or equal to the allowable torque of the reduction gear is calculated.

In step ST18, first, an orthogonal coordinate value and a joint displacement q _c3 of a point (point C3) that returns to the operation start point (acceleration start point) A at a linear velocity v ₁ from the end point of acceleration for a minute time Δt are calculated. Further, the Cartesian coordinate value and the joint displacement _qd3 of the point (point D3) that has advanced from the deceleration start point to the operation end point (deceleration end point) B at the linear velocity v ₁ for a minute time Δt are calculated. Next, the speeds v _1c and v _2c are calculated by the equations (10) and (11), respectively.
v _1c = (q _c −q _c3 ) / Δt (10)
v _2c = (q _d3 −q _d ) / Δt (11)
Furthermore, the accelerations a _c and a _d at the acceleration end point and the deceleration start point can be calculated by equations (12) and (13), respectively.
a _c = (v _1b −v _1c ) / Δt (12)
a _d = (v _2c −v _2b ) / Δt (13)
Accordingly, the motor torque τ _c and the speed reducer torque τ _lc at the acceleration end point are expressed by equations (14) and (15), respectively.
τ _c = M _c a _c + h _c (14)
τ _lc = M _lc a _c + h _lc (15)
Here, M _c , h _c , M _lc , and h _lc are M and h in equation ( ₁ ) and M _l and h _{l in} equation (9) calculated from joint displacement q _c and joint velocity v _1b at the acceleration end point, respectively. It is.

Next, when the linear velocity at the acceleration end point is multiplied by k only at the same location, the velocity of equations (6) and (10) and the acceleration of equation (12) are also multiplied by k. For simplicity, the change in the centrifugal-Coriolis force due to speed changes is negligible among the h _c, the viscous friction force f _vc, when the remainder and h _2c, linear velocity in the acceleration end point is the same When only the speed is multiplied by k at the place, the above equations (14) and (15) are transformed into the following equations (16) and (17), respectively.
τ _c = k ^* (M _{c ac} + f _vc ) + h _2c (16)
τ _lc = k ^* M _lc a _c + h _lc (17)

The maximum k _a is calculated in _a range that satisfies the constraint that the calculation results of the equations (16) and (17) are equal to or less than the allowable torque of the motor and the reduction gear for any axis. Equation (16), (17) Since the first-order equation relating k _{k a} can be easily calculated. For example, the maximum value _{k mi} of k satisfying restrictions tau _Mmaxi the motor torque of the _i-axis, when the _{(M c a c + f vc} ), respectively the i-th row element of _{h 2c m} i, _{h i, m i} In the case of> 0, Expression (18) is obtained.
_{k mi = (τ mmaxi -h i} ) / m i (18)
Further, when m _i <0, (19) is obtained.
_{k mi = (- τ mmaxi -h} i) / m i (19)
Further, when m _i = 0, the motor torque limit τ _mmaxi is satisfied at an arbitrary k, and thus a large positive value is set in advance. Calculating the minimum value as the speed ratio k _a from the maximum value of k satisfying torque limit of the maximum value of k satisfying the motor torque limit of all axes and reduction gear.
Even at the deceleration start point, the acceleration, speed, and joint displacement at the deceleration start point are exactly the same as the calculation at the acceleration start point, and the maximum within the range that satisfies the constraints that are below the allowable torque of the motor and reducer on any axis calculate the velocity ratio k _b is the k.

Then calculated by the following equation (20) the speed limit parameter _{v a} at the acceleration end point from the current speed limit parameters vmax and speed ratio _{k a.
^{v a = vmax * k a (}} 20)
Furthermore the speed limit parameter at the deceleration start point from the current speed limit parameters vmax and k _b is calculated by equation (21).
v _b = vmax ^* k _b (21)

  As described above, according to the first embodiment, the robot parameter storage unit 6 stores the maximum allowable speed of each axis of the robot, the maximum reference speed in the orthogonal coordinate system, and the maximum allowable torque of the motor that drives each axis. Value, the maximum allowable torque of the transmission torque of each axis and the moment, and the orthogonal coordinate values and joint displacements of the acceleration start point and deceleration end point inputted by the reference speed calculation means 2, Based on the allowable maximum speed of the axis or the reference maximum speed in the Cartesian coordinate system, calculate the reference speed that is reached when the acceleration time elapses from the start of movement in each axis or Cartesian coordinate system for the current movement of the robot. In the deceleration time / speed upper limit parameter determination means 7, it responds to at least one of the orthogonal coordinate value and the joint displacement of the input acceleration start point and deceleration end point. As long as the maximum allowable torque of the motor that drives each axis for each operation, the maximum allowable torque and moment of the transmission mechanism of each axis, and the maximum allowable speed of each axis are satisfied. The shortest acceleration time, deceleration time, and maximum speed upper limit parameter are calculated, and the command curve generation means 3 calculates the robot's speed based on the calculated reference speed, acceleration time, deceleration time, and maximum speed upper limit parameter of the current operation. A command curve for controlling the motor that drives each axis is generated. Therefore, since the optimum speed upper limit value and acceleration / deceleration parameters can be set for each operation, the effect of increasing the speed of the robot can be obtained.

Further, as a concrete example for this purpose, in the first embodiment, the acceleration / deceleration time / speed upper limit parameter determination means 7 performs the calculation at the specified number of repetitions, and the orthogonal coordinate values of the given acceleration start point and deceleration end point. Orthogonal coordinate values at the acceleration end point and the deceleration start point based on the reference speed in each direction in the operation calculated by the reference speed calculation means 2 and the current acceleration time, deceleration time and speed upper limit parameters, joint displacement Calculate the speed in each direction and the speed of each axis, and calculate the shortest acceleration time for each axis at the acceleration start point and acceleration end point within the range that satisfies the allowable torque limit of the motor and transmission mechanism. , shortest maximum value _t 1a of acceleration time obtained in each _point, the _{t 1b} respectively extracted, extracted these maximum values _{t 1a,} in _{t 1b,} large This value is determined as the current acceleration time, and the shortest deceleration time for each axis is calculated at each of the deceleration start point and deceleration end point within the range that satisfies the allowable torque limit of the motor and transmission mechanism. Then, the maximum values t _2a and t _2b of the shortest deceleration time at each of the obtained points are extracted, and the larger value among the extracted maximum values t _2a and t _2b is the current deceleration time. From each joint displacement at the acceleration end point and the deceleration start point, and the speed in each direction, the constant speed motion can be continued within the range that satisfies the constraints of the allowable torque of the motor and transmission mechanism of each axis. maximum speed limit parameter v _a, v _b were calculated, the calculated these maximum speed limit parameter v _a, v _b and in the current speed limit parameter, the minimum value of the current It is characterized in that so as to determine a degree bound parameter.

Embodiment 2. FIG.
The configuration of the robot control apparatus according to the second embodiment is also shown in FIG. 1, and the flow of processing up to the determination of the acceleration time, the deceleration time, and the speed upper limit parameter in the acceleration / deceleration time / speed upper limit parameter determination means 7 is also the flowchart of FIG. It is represented by Since only the speed upper limit parameter calculation method in step ST18 is different from the first embodiment, the operation in step ST18 will be described.
Step acceleration a _c on the acceleration end point in the interior of _ST18, until the calculation of the acceleration a _d of the deceleration start point is the same as the first embodiment. In the first embodiment, the effect of changing the centrifugal / Coriolis force when the speed is multiplied by k is ignored. However, in the second embodiment, the change in the centrifugal / Coriolis force is also taken into consideration. The maximum speed magnification within a range satisfying the allowable torque limit is calculated.

First, assuming that the centrifugal displacement / Coriolis force calculated from the joint displacement q _c and the joint velocity v _1b at the acceleration end point is c _c , the gravity is g _c , and the coulomb friction force is f _cc , the above equations (14) and (15) Can be transformed into the following equations (22) and (23), respectively.
τ _c = k ^{2 *} c _c + k ^* (M _{c ac} + f _vc ) + (g _c + f _cc ) (22)
τ _lc = k ^{2 *} c _c + k ^* M _lc a _c + g _c (23)
Equation (22), (23) Since the quadratic equation of the speed ratio k, k ² and performs why if k symbols each of the coefficients in the maximum speed ratio k in the range satisfying the torque tolerance value of each axis motor If, to calculate the rate factor k that maximizes the extent to satisfy the constraints of the allowable torque of the reduction gear of each shaft, when the minimum value of the a k _a, the allowable torque of the motor and reduction gear in both axes hereinafter become constraint satisfies a range at the maximum speed ratio k _a is calculated.

Similarly joint displacement _{q d} of the deceleration start point, the centrifugal-Coriolis force calculated from the joint velocity _{v 2a c d,} gravity _{g d,} the Coulomb friction force and _{f cd,} joint displacement _q d of the deceleration start point, If M and h in the equation (1) calculated from the joint velocity v _2a and M _l and h _l in the equation (9) are M _d , h _d , M _ld and h _ld , respectively, the motor torque at the deceleration start point And the reduction gear torque is as shown in the following equations (24) and (25), respectively.
τ _d = k ^{2 *} c _d + k ^* (M _d a _d + f _vd ) + (g _d + f _cd ) (24)
^{_{τ ld = k 2 * c d}} + k * M ld a d + g d (25)
Equation (24), so (25) is a quadratic equation of the speed ratio k, performed not when each sign of the coefficient of k ² and k, the maximum speed ratio k in the range satisfying the motor torque tolerance value for each axis calculates the speed ratio k that maximizes the extent to satisfy the constraints of the allowable torque of the reduction gear of each shaft, when the minimum value of the the speed ratio k _a, allowable motor and reduction gear in both axes maximum speed ratio k _b is calculated in a range that satisfies the constraints become less torque.
Above manner, the speed ratio k _a that is calculated _from the k _b, as in the first embodiment formula (20), the speed limit parameter v _a and the deceleration start point at the acceleration end point using (21) speed limit parameter v _b in is calculated.

As described above, according to the second embodiment, the acceleration / deceleration time / speed upper limit parameter determining means 7 replaces the portion for obtaining the speed upper limit parameter of the first embodiment with the acceleration end point and the deceleration start point. Based on the centrifugal / Coriolis force calculated from each joint displacement and joint speed, gravity, and Coulomb friction force, the maximum value within the range that satisfies the limit of the allowable torque of each axis motor at each point (acceleration end point and deceleration start point) become speed magnification and the maximum and becomes the speed ratio in a range that satisfies the constraints of the allowable torque of the speed reducer of each axis is calculated respectively, in both the speed ratio at each point is calculated, the minimum speed ratio k _a, k _b is extracted, respectively, the speed ratio k _a of the extracted minimum values at each _point, based on the k _b, the maximum speed limit parameter v _a a speed limit parameter at the deceleration start point at the acceleration end point v _b is calculated, and among the calculated maximum speed upper limit parameters v _a and v _b and the current speed upper limit parameter, the minimum value is determined as the current speed upper limit parameter. . Therefore, the maximum speed multiplication factor can be calculated in a range that satisfies the restriction of the allowable torque of the motor and the reduction gear in consideration of the change in centrifugal force and the Coriolis force. The effect that a parameter can be set is obtained.

Embodiment 3 FIG.
The configuration of the robot control apparatus according to the third embodiment is also shown in FIG. The flow of processing from the acceleration / deceleration time / speed upper limit parameter determination means 7 to the acceleration time, deceleration time and speed upper limit parameter determination is shown in the flowchart of FIG. In FIG. 3, the third embodiment is different from the first and second embodiments in the method of calculating the speed upper limit parameter in step ST18, and steps ST22 and ST23 are newly added. It is different in point. Therefore, different points will be mainly described.
First, in step ST18, the maximum speed magnification ka is calculated in _a range that satisfies the allowable torque constraints of all motors and decelerators at the acceleration end point, and the allowable torques of all motors and decelerators are calculated at the deceleration start point. calculate the velocity ratio k _b which maximizes the extent that satisfy the constraints. Calculation method of the speed ratio k _a and k _b is exactly same as in the second embodiment.

Next, a magnification k _a2 that is maximum in a range satisfying the motor speed limit is calculated from the speed v _1b at the acceleration end point. _Assuming that the i-th element of v _1b is v _1bi and the motor allowable maximum speed of the i-th axis is vel _i , the ratio vr _i of the i-th axis speed to the maximum speed can be calculated by Expression (26).
vr _i = abs (v _1bi ) / vel _i (26)
Here, abs () means an absolute value. If the maximum value of vr _i at the acceleration start point is vrmax _a , the maximum magnification k _a2 within the range satisfying the limit of the motor speed can be calculated by the following equation (27).
k _a2 = 1 / vrmax _a (27)
Similarly, to calculate the ratio k _b2 becomes maximum in a range satisfying the restriction from the speed v _2a of the motor speed of the deceleration start point.
The smaller of k _a and k _a2 is k _a3 , and the smaller of k _b and k _b2 is k _b3, and the speed upper limit parameters v _a and v _b at the acceleration end point are respectively calculated from the current speed upper limit parameter vmax by the following equations: Calculated in (28) and (29).
^{_{v a = vmax * k a3 (}} 28)
v _b = vmax ^* k _b3 (29)

Next, steps ST22 and ST23 will be described.
First, it is determined whether or not there is a constant speed section when the acceleration time, deceleration time, and speed upper limit value calculated up to step ST20 are used (step ST22). In determining whether there is a constant speed section, it can be determined that there is a constant speed section when the sum of the movement distances of the acceleration section and the deceleration section is shorter than the movement distance of the operation, while there is no constant speed section when it is long. .

If it is determined in step ST22 that “there is a constant speed section”, the speed upper limit parameter is corrected at the midpoint of the constant speed section (step ST23). In this case, the midpoint of the constant speed section, i.e. determine the midpoint (M1 points) of the deceleration start point and the acceleration end point, and calculates joint displacement q _m midpoint. Next, the joint displacement q _m2 at the point (M2 point) that has advanced from the midpoint (M1 point) of the constant velocity section to the operation end point B for the minute time Δt at the speed of the constant velocity section, and the midpoint ( A joint displacement q _m3 is calculated at a point (point M3) that returns from the point M1) to the operation start point A for a minute time Δt at a speed in the constant velocity section. Furthermore the speed _{v m} and the acceleration _{a m} in the midpoint (M1 points) Equation (30), (31), is calculated by (32).
v _m = (q _m2 −q _m ) / Δt (30)
v _m2 = (q _m −q _m3 ) / Δt (31)
a _m = (v _m −v _m2 ) / Δt (32)

At the midpoint (M1 points), all the motor and reduction gear permissible maximum speed ratio k _ma in the range satisfying the constraint torque of the shaft, the allowable motor and reduction gear of all axes at the acceleration end point at step ST18 obtaining at exactly the same manner as the calculation of the maximum speed ratio k _a range that satisfies the constraints of the torque. Further, the maximum speed multiplication factor k _mv within the range satisfying the limitation on the maximum motor speed at the midpoint is exactly the same as the calculation of the maximum speed magnification factor k _a2 within the range satisfying the limitation on the maximum motor speed at the acceleration end point in step ST18. Ask for it. Next, a smaller speed ratio _{k ma} and _{k mv} calculated as _{k m,} in the case of _k m <1 rewrites the speed limit parameter _k ^m * vmax. In addition, when k _m ≧ 1, the speed upper limit parameter is not rewritten.
If it is determined in step ST22 that “there is no constant velocity section”, the parameter is not corrected and the operation is terminated.

As described above, according to the third embodiment, the acceleration / deceleration time / speed upper limit parameter determining means 7 replaces the portion for obtaining the speed upper limit parameter of the first and second embodiments with reference to the acceleration end point. Based on the centrifugal displacement, Coriolis force, gravity, and Coulomb friction force calculated from the joint displacement and joint speed at each deceleration start point, the maximum speed magnification within the range that satisfies the allowable torque limit of each axis motor at each point respectively the maximum and becomes the speed ratio in a range that satisfies the constraints of the allowable torque of the speed reducer of each axis is calculated respectively, in both the speed ratio at each point is calculated, the minimum speed ratio k _a, k _b and extracted, each of the velocity v _1b in the acceleration end point, and the deceleration start _point, the speed ratio from v _2a becomes maximum in a range satisfying the restriction of the motor speed k _{a2, k} b2 _were calculated respectively The minimum speed ratio _k a, _{k b} and based on the value _{k a3, k b3} of the smaller among the speed ratio _{k a2, k b2} becomes maximum, the speed limit parameter at the deceleration start point and the acceleration end point _{v a} , V _b are calculated respectively. Also, when using the acceleration time, deceleration time, and speed upper limit parameters, it is determined whether there is a constant speed section. If there is a constant speed section, the speed upper limit parameter is corrected at the midpoint of the constant speed section. calculates a joint displacement q _m midpoint, then the joint displacement q _{m @ 2} point advanced by the operation end point-side minute time Δt at the speed of constant speed section from the midpoint of the constant speed section, constant speed section the joint displacement q _m3 point returned only to the operation start point-side speed minute time of constant speed section Δt is calculated from the midpoint, further calculates a velocity v _m and the acceleration a _m in the midpoint, in the midpoint Calculate the maximum speed multiplication factor k _ma within the range that satisfies the restrictions on the allowable torque of the motors and reduction gears of all axes, and calculate the maximum speed multiplication factor k _mv within the range that satisfies the limitation of the maximum motor speed at the midpoint. Of the calculated speed magnifications k _ma and k _mv In smaller value k _m is only smaller than 1, based on the smaller value (the speed ratio k _m), is characterized in that so as to rewrite each speed limit parameter k _m ^* vmax. Therefore, even in the case where there is a constant speed section, the effect that the optimum speed upper limit parameter can be set for each operation can be obtained as in the first embodiment.

Embodiment 4 FIG.
4 is a block diagram showing a configuration of a robot control apparatus according to Embodiment 4 of the present invention. In the figure, the same or corresponding parts as in FIG. 1 are denoted by the same reference numerals, and the description thereof will be omitted in principle.
The reference speed calculation means 20 in the fourth embodiment is a means for calculating a reference speed in each direction in each axis or Cartesian coordinate system for the current operation of the robot, like the reference speed calculation means 2 in FIG. When the movement command to move from point A to point B is joint interpolation that operates each joint synchronously, the reference speed in each axis for the operation is calculated based on the allowable maximum speed for each axis of the robot. To do. When the movement command for moving from point A to point B is linear interpolation that operates on a straight line, the reference speed in each direction in the orthogonal coordinate system is calculated based on the reference maximum speed in the orthogonal coordinate system. To do. The command curve generating means 30 is a means for generating a command curve in the same manner as the command curve generating means 3 in FIG. 1. In this case, the maximum speed of the operation calculated by the reference speed calculating means 20 and the operation parameter are used. A command curve is generated based on the operation parameter corrected by the correction means 12.

  The acceleration / deceleration time / speed upper limit parameter determination means 70 determines the maximum allowable driving torque of the motor stored in the robot parameter storage means 6 for each operation when the movement command for moving from the point A to the point B is joint interpolation. The shortest acceleration time and deceleration time are calculated in a range that satisfies the constraints of the allowable value of the value, the acting torque of the transmission mechanism of each axis and the moment. In addition, when the movement command for moving from point A to point B is linear interpolation, it is the shortest in the range that satisfies the constraints of the allowable maximum value of the motor drive torque, the operating torque of the transmission mechanism of each axis, and the allowable maximum value of the moment. Acceleration time, deceleration time and maximum speed upper limit parameters are calculated. The calculation method of the acceleration time and the deceleration time in the case of joint interpolation is the same as the method by the acceleration / deceleration time determination means of the above-mentioned Patent Document 1. Further, the calculation method of the acceleration time, the deceleration time, and the speed upper limit parameter in the case of linear interpolation is the same as the method by the acceleration / deceleration time / speed upper limit parameter determination means 7 of the first embodiment.

The torque usage rate calculating means 10 is the acceleration time kt _mi and the acceleration / deceleration time / speed upper limit that are the shortest in the range satisfying the allowable torque limit of the motor of each axis calculated in the acceleration / deceleration time / speed upper limit parameter determining means 70. From the acceleration time kt finally obtained by the parameter determining means 70, the torque usage rate rate _i of the acceleration section of each axis is calculated by the following equation (33).
tratek _i = kt _mi / kt (33)
Similarly, the torque usage rate in the deceleration section of each axis from the deceleration time gt _mi that is the shortest within the range satisfying the motor torque limit of each axis and the deceleration time gt finally obtained by the acceleration / deceleration time / speed upper limit parameter determining means 70 The rate _i is calculated by the following equation (34).
rateg _i = gt _mi / gt (34)

The correction rate storage means 11 stores a robot operation command and stores a set value hose _i of the correction rate for each axis described in a program processed by the robot controller. The operation parameter correction unit 12 calculates the acceleration / deceleration time / speed upper limit parameter determination unit 70 using the torque usage rate calculated by the torque usage rate calculation unit 10 and the correction rate of each axis stored in the correction rate storage unit 11. The acceleration time and deceleration time thus corrected are corrected as follows and output to the command curve generating means 30.

In the operation parameter correction unit 12, first, a correction candidate value is obtained from the torque usage rate in the acceleration section of each axis obtained by the torque usage rate calculation unit 10 and the setting value of the correction rate of each axis stored in the correction rate storage unit 11. aratek _i is calculated by the following equation (35).
aretek _i = tratek _i ^* 100 / hose _i (35)
A maximum value aratekmax is obtained from the calculated correction candidate values aratek _i . Here, if artekmax ≦ 1, the acceleration time is not corrected, and if artekmax> 1, the acceleration time kt is rewritten to kt ^* artekmax.
Similarly, to calculate a correction candidate value Arateg _i by the following equation (36) from the set value of the torque utilization and correction factor for each axis of the deceleration zone.
arteg _i = rateg _i ^* 100 / hose _i (36)
A maximum value arategmax is obtained from the calculated correction candidate values arategi _i . When arategmax ≦ 1, the deceleration time is not corrected. When arategmax> 1, the deceleration time gt is rewritten to gt ^* arategmax. In the case of linear interpolation, the speed upper limit parameter calculated by the acceleration / deceleration time / speed upper limit parameter determining means 70 is output to the command curve generating means without correction.

  As described above, according to the fourth embodiment, the acceleration / deceleration time / speed upper limit parameter determining means 70 responds to at least one of the input orthogonal coordinate values of the acceleration start point and the deceleration end point and the joint displacement. In the case of joint interpolation, the permissible maximum value of the driving torque of the motor that drives each axis, the operating torque of the transmission mechanism of each axis, and the permissible maximum value of the moment stored in the robot parameter storage means 6 for each operation. Calculate the shortest acceleration time and deceleration time within a range that satisfies at least one constraint. In the case of linear interpolation, the maximum allowable drive torque of the motor, the maximum allowable torque of the transmission mechanism of each axis, and the maximum allowable moment The shortest acceleration time, deceleration time, and maximum speed upper limit parameter are calculated within a range that satisfies at least one of the constraints, and the torque usage rate calculation means 10 Based on the shortest acceleration time and the shortest deceleration time calculated by the speed upper limit parameter determining means 70, the torque usage rate of the acceleration section of each axis and the torque usage rate of the deceleration section of each axis are calculated, and each calculated The acceleration / deceleration time / speed upper limit parameter determination means 70 is operated by the operation parameter correction means 12 on the basis of the torque usage rate in the acceleration and deceleration sections of the shaft and the correction ratio for each axis stored in the correction ratio storage means 11. Since the calculated acceleration time and deceleration time are corrected, it is possible to obtain an effect that the operation parameters can be automatically corrected according to the ratio specified in advance and the motor load of each axis. Here, the correction of the operation parameter need not be limited to the acceleration time and the deceleration time. The determination of the acceleration / deceleration time / speed upper limit parameter need not be limited to the case where the speed upper limit parameter is also calculated. In the case where the speed upper limit parameter is not calculated, the acceleration / deceleration time / speed upper limit parameter determining means 70 responds to at least one of the input Cartesian coordinate values of the acceleration start point and the deceleration end point and the joint displacement for each operation. Acceleration / deceleration for calculating the shortest acceleration time and deceleration time within a range satisfying at least one of the allowable maximum value of the driving torque of the motor that drives each axis, the maximum allowable torque value of the transmission torque of each axis, and the moment What is necessary is just to replace with a determination means.

As a specific process, in the fourth embodiment, the torque usage rate calculating means 10 is the shortest within a range that satisfies the constraint on the allowable torque of the motor of each axis calculated by the acceleration / deceleration time / speed upper limit parameter determining means 70. Based on the acceleration time kt _mi , the deceleration time gt _mi and the finally obtained acceleration time kt and deceleration time gt, the torque usage rate of the acceleration zone and the deceleration zone of each axis is calculated, and the operation parameter correction means 12 uses the torque. Based on the torque usage rates of the acceleration and deceleration zones of each axis determined by the rate calculation means 10 and the set values of the correction rates of the respective axes stored in the correction rate storage means 11, correction candidate values for the respective sections are obtained. The maximum value is calculated from the calculated correction candidate values, and only when the calculated maximum value is larger than 1, the finally determined acceleration time kt and deceleration time gt are obtained. It is characterized in based on the maximum value of greater than one correction candidate value that was replaced in each ^kt * ^aratekmax and ^gt * ^arategmax.

Embodiment 5 FIG.
The configuration of the robot control apparatus according to the fifth embodiment is also shown in FIG. Since the operation parameter correcting means 12 and the command curve generating means 30 are different from those of the fourth embodiment, only the different parts will be described.
First, the operation parameter correction means 12 calculates a correction candidate value ratek _i by the following equation (37) from the torque usage rate in the acceleration section and the set value of the correction rate.
ratek _i = tratek _i ^* 100 / hose _i (37)
A maximum value ratekmax is determined from the calculated correction candidate values ratek _i .
Similarly, a correction candidate value vrateig _i is calculated by the following equation (38) from the set value of the torque usage rate and the correction rate in the deceleration zone.
ratei _i = rateg _i ^* 100 / hose _i (38)
A maximum value ratemaxmax is obtained from the calculated correction candidate value raterate _i .

Next, a larger one of the maximum correction values vratekmax and rategmax of the two correction candidate values is set as a final correction candidate value ratemax. When ratemax> 1, the reference speed correction rate ovrd is calculated by the following equation (39).
ovrd = 1 / vratemax (39)
When ratemax ≦ 1, ovrd = 1. To the command curve generation means 30, the reference speed correction rate ovrd and the acceleration time, deceleration time and speed upper limit parameters calculated by the acceleration / deceleration time / speed upper limit parameter determination means 70 are output.
The command curve generation means 30 generates a command curve using a value obtained by multiplying the reference speed calculated by the reference speed calculation means 20 by the reference speed correction rate ovrd calculated by the operation parameter correction means 12 as a reference speed.

As described above, according to the fifth embodiment, the torque usage rate calculation means 10 is the shortest in a range that satisfies the constraint on the allowable torque of the motor of each axis calculated by the acceleration / deceleration time / speed upper limit parameter determination means 70. Based on the acceleration time kt _mi , the deceleration time gt _mi and the finally obtained acceleration time kt and the deceleration time gt, the torque usage rate of the acceleration zone and the deceleration zone of each axis is calculated, and the operation parameter correction means 12 The correction candidate value is calculated based on the calculated torque usage rate and correction rate setting values for the acceleration and deceleration intervals of each axis, and the correction candidate value for each interval is calculated from the calculated correction candidate values. The reference speed correction rate is calculated based on the larger value among the extracted maximum values in each section, and the command curve generation means uses the reference speed calculation means 20. By multiplying the calculated reference speed correction rate calculated in the reference speed was modified reference speed, is characterized in that so as to generate the command curve. Therefore, as in the fourth embodiment, there is an effect that the operation parameter can be automatically corrected according to the ratio specified in advance and the load of the motor of each axis.

Embodiment 6 FIG.
The configuration of the robot control apparatus according to the sixth embodiment is also shown in FIG. Since the operations of the operation parameter correction unit 12 and the command curve generation unit 30 are different from those of the fourth embodiment, only the different parts will be described.
First, the operation parameter correction means 12 calculates a correction candidate value dratek _i from the following equation (40) from the torque usage rate in the acceleration section and the set value of the correction rate.
ratek _i = trate _i ^* 100 / hose _i (40)
A maximum value dratekmax of the calculated correction candidate value dratek _i is obtained. Similarly the torque utilization of the deceleration zone from the correction factor setting value to calculate a correction candidate value Drateg _i from the following equation (41).
drate _i = rate _i ^* 100 / hose _i (41)
Find the maximum value drategmax the calculated correction candidate value drateg _i.
Next, the larger one of dratekmax and drategmax is defined as dratemax. When the ratemax> 1, the waiting time correction rate rdelay is set to rdelay = dratemax (42)
Calculate with When delaymax ≦ 1, rdelay = 1 is set.

  After calculating the waiting time correction rate rdelay, the operation parameter correction unit 12 outputs the value to the command curve generation unit 30 together with the acceleration time, deceleration time, and speed upper limit parameters calculated by the acceleration / deceleration time / speed upper limit parameter determination unit 70. . In the command curve generation means 30, when a robot operation command is described and a waiting time is specified between operations on the program processed by the robot controller, the waiting time is corrected to the specified waiting time. Correction is made by multiplying by the rate rdelay, and the operation is shifted to the next operation after stopping for the corrected waiting time.

As described above, according to the sixth embodiment, the torque usage rate calculating means 10 is the shortest in the range satisfying the restriction on the allowable torque of the motor of each axis calculated by the acceleration / deceleration time / speed upper limit parameter determining means 70. Based on the acceleration time kt _mi , the deceleration time gt _mi and the finally obtained acceleration time kt and the deceleration time gt, the torque usage rate of the acceleration zone and the deceleration zone of each axis is calculated, and the operation parameter correction means 12 The correction candidate value is calculated based on the calculated torque usage rate and correction rate setting values for the acceleration and deceleration intervals of each axis, and the correction candidate value for each interval is calculated from the calculated correction candidate values. And the waiting time correction rate is calculated based on the larger one of the extracted maximum correction candidate values in each section. When waiting time is specified between actions, the robot operation instruction is described and corrected by multiplying the waiting time described on the program processed by the robot controller by the calculated waiting time correction rate, A command curve that shifts to the next operation after generating an operation stop according to the corrected waiting time is generated. Therefore, as in the fourth embodiment, there is an effect that the operation parameters can be automatically corrected according to the ratio designated in advance and the motor load of each axis.

Embodiment 7 FIG.
The configuration of the robot control apparatus according to the seventh embodiment is also shown in FIG. Since the operation of the torque usage rate calculation means 10 is different from that of the fourth embodiment, only the different parts will be described.
The torque usage rate calculating means 10 is a sum h _c of inertia matrix M _c , joint speed v _1b , centrifugal / Coriolis force, gravity, frictional force at the acceleration end point calculated in the acceleration / deceleration time / speed upper limit parameter determining means 70. From the acceleration time kt, the motor torque τ _mk at the acceleration end point is calculated by the following equation (43).
τ _mk = M _c v _1b / kt + h _c (43)
The i-th axis element of the motor torque τ _mk at the acceleration end point is τ _mki, and the torque usage rate tratek _i in the acceleration section of each axis is calculated from the following equation (44).
tratek _i = abs (τ _mki ) / τ _maxmi (44)
Here, τ _maxmi is the allowable maximum torque of each axis motor. Similarly, the inertia matrix M _d at the deceleration start point calculated by the internal deceleration time and speed limit parameter determining means _70, the joint velocity v _2a, centrifugal-Coriolis force, gravity, from the deceleration time gt sum h _d of frictional force, The motor torque τ _mg at the deceleration start point is calculated by the following equation (45).
τ _mg = −M _d v _2a / gt + h _d (45)
The i-th axis element of the motor torque τ _mg at the deceleration start point is set to τ _mgi, and the torque usage rate rate _i in the deceleration section of each axis is calculated from the following equation (46).
rateig _i = abs (τ _mgi ) / τ _maxmi (46)
The calculated torque usage rate in the acceleration zone and the torque usage rate in the deceleration zone of each axis are output to the operation parameter correction unit 12, and the operation parameter correction unit 12 corrects the acceleration time and the deceleration time as in the fourth embodiment. Used for.

As described above, according to the seventh embodiment, the torque usage rate calculation means 10 calculates the inertia matrix, joint speed, centrifugal speed at the acceleration end point and the deceleration start point calculated by the acceleration / deceleration time / speed upper limit parameter determination means 70. Calculate motor torques τ _mk , τ _mg at each point based on the sum of Coriolis force, gravity, friction force and acceleration time kt, deceleration time gt, and motor torques τ _mk , τ _mg and motors of each axis Based on the permissible maximum torque, the torque usage rates of the acceleration zone and the deceleration zone of each axis are calculated, and the operation parameter correction means 12 stores the calculated torque usage rates and the correction rate of the acceleration zone and the deceleration zone of each axis. The correction candidate value of each section is calculated based on the set value of the correction factor of each axis stored in the means, and used for correcting the acceleration time and the deceleration time. That. Therefore, it is possible to obtain an effect that the operation parameter can be automatically corrected according to the ratio specified in advance and the load of the motor of each axis.

It is a block diagram which shows the structure of the robot control apparatus by Embodiment 1 thru | or Embodiment 3 of this invention. It is a flowchart which shows the process sequence of the acceleration / deceleration time and speed upper limit parameter determination means which concerns on Embodiment 1 and Embodiment 2 of this invention. It is a flowchart which shows the process sequence of the acceleration / deceleration time and speed upper limit parameter determination means based on Embodiment 3 of this invention. It is a block diagram which shows the structure of the robot control apparatus by Embodiment 4 thru | or Embodiment 7 of this invention.

Explanation of symbols

  DESCRIPTION OF SYMBOLS 1 Teaching point memory | storage means, 2,20 Reference speed calculating means, 3,30 Command curve generation means, 4 Motor control means, 5 Robot, 6 Robot parameter memory | storage means, 7,70 Acceleration / deceleration time / speed upper limit parameter determination means, 10 Torque usage rate calculation means, 11 correction rate storage means, 12 operation parameter correction means.

Claims (2)

The maximum allowable speed of each axis of the robot, the reference maximum speed in the Cartesian coordinate system, the allowable maximum value of the driving torque of the motor that drives each axis, the allowable torque of the transmission mechanism of each axis and the allowable maximum value of the moment are stored in advance. Robot parameter storage means,
Each axis or Cartesian coordinate for the current motion of the robot based on the input Cartesian coordinate value and joint displacement of the acceleration start point and deceleration end point and the allowable maximum speed of each axis or the reference maximum speed in the Cartesian coordinate system A reference speed calculation means for calculating a reference speed that is reached when an acceleration time has elapsed since the start of operation in each direction in the system;
In response to at least one of the orthogonal coordinate value and the joint displacement of the input acceleration start point and deceleration end point, the allowable maximum value of the drive torque of the motor that drives each axis for each operation, the transmission mechanism of each axis Acceleration / deceleration time / speed upper limit parameter determination means for calculating the shortest acceleration time, deceleration time and maximum speed upper limit parameter within a range satisfying at least one restriction of the allowable maximum value of the applied torque and moment and the allowable maximum speed of each axis. When,
Robot control comprising command curve generating means for generating a command curve for controlling a motor for driving each axis of the robot based on the calculated reference speed, acceleration time, deceleration time and maximum speed upper limit parameter of the current operation apparatus. The maximum allowable speed of each axis of the robot, the reference maximum speed in the Cartesian coordinate system, the allowable maximum value of the driving torque of the motor that drives each axis, the allowable torque of the transmission mechanism of each axis and the allowable maximum value of the moment are stored in advance. Robot parameter storage means,
If the movement from the acceleration start point to the deceleration end point is joint interpolation in response to the Cartesian coordinate value and the joint displacement of the input acceleration start point and deceleration end point, the maximum allowable speed of each axis of the robot is set. Based on this, the reference speed reached when the acceleration time has elapsed since the start of each axis of the current movement is calculated, and when the movement from the acceleration start point to the deceleration end point is linear interpolation, the reference in the orthogonal coordinate system is calculated. A reference speed calculation means for calculating a reference speed that is reached when the acceleration time has elapsed from the start of operation in each direction of the orthogonal coordinate system of the current operation based on the maximum speed;
In response to at least one of the orthogonal coordinate value and the joint displacement of the input acceleration start point and deceleration end point, the allowable maximum value of the drive torque of the motor that drives each axis for each operation, the transmission mechanism of each axis Acceleration / deceleration time determining means for calculating the shortest acceleration time and deceleration time within a range satisfying at least one constraint of the allowable maximum value of the applied torque and moment;
Torque usage rate calculating means for calculating the torque usage rate of the acceleration section of each axis based on the calculated shortest acceleration time, and calculating the torque usage rate of the deceleration section of each axis based on the minimum deceleration time. ,
Correction rate storage means for storing in advance the correction rate of each axis of the robot;
An operation parameter correcting means for correcting an operation parameter based on the calculated torque usage rate in each acceleration and deceleration interval of each axis and the correction rate for each axis;
A robot control device comprising command curve generation means for generating a command curve for controlling a motor for driving each axis of the robot based on the calculated reference speed, acceleration time, deceleration time and the corrected operation parameters.

Images