JP7510338B2 - Speed command value calculation device - Google Patents

Description

This specification relates to a speed command value calculation device that calculates a speed command value using moving average processing.

Working robots are widely used to promote automation and labor saving in factories and business establishments. Many working robots are equipped with a movable arm, an end effector attached to the tip of the movable arm to perform a specified task, and a drive mechanism to drive the movable arm. To achieve the movement required of the movable arm, a speed command value calculation device is used to calculate a command value to be issued to the drive mechanism. One type of speed command value calculation device is one that uses moving average processing. Examples of technology related to this type of speed command value calculation device are disclosed in Patent Documents 1 and 2.

The command generation device in Patent Document 1 includes a moving average filter that calculates the moving average of the speed command in a predetermined average time and outputs it as a moving average speed command, a means for changing the average time, a means for calculating the difference in moving distance that occurs with the change in the average time as a moving distance correction amount, and a means for calculating a speed correction value according to the moving distance correction amount, and calculates an operation command in the positioning control of the electric motor by adding the speed correction value to the moving average speed command. According to this, it is said that it is possible to change the average time while the electric motor is operating and to perform correct positioning without providing multiple moving average filters.

The position command device of Patent Document 2 includes a linear acceleration/deceleration command generation unit that generates a first command position from a command of a higher-level system, an S-curve acceleration/deceleration command generation unit that generates a second command position by taking a moving average of a settable number of times of the first command position, a transmission unit that transmits the second command position to the position control device, and a number change determination unit that generates a number change command when the number can be changed according to a command to change the moving average number of times from the higher-level system. It is said that this makes it possible to provide a position command device that is capable of changing the moving average time.

International Publication No. 2011/101915 JP 2008-171230 A

The technologies in Patent Documents 1 and 2 aim to reduce mechanical vibrations and shorten operating times at the same time by varying the averaging parameters (average time, average number of moving averages) of the moving average. In other words, when smooth movement is required, the averaging parameters are changed significantly to limit speed changes, and when shortening the operating time is required, the averaging parameters are changed smaller to allow larger speed changes. However, these technologies are intended to improve the movement of the moving part under normal conditions, and are not intended to address abnormal conditions.

In more detail, when the moving part moves in accordance with the command values (operation command, second command position), it is possible that an obstacle may occur. The techniques of Patent Documents 1 and 2 cannot deal with such abnormal conditions that require an emergency stop. In other words, with the techniques of Patent Documents 1 and 2, even if an abnormal condition occurs, the moving part stops at the same deceleration as in normal times. As a result, problems occur, such as the moving part colliding with an obstacle before stopping.

On the other hand, in order to deal with abnormal conditions, it is possible to use an immediate stopping means that sets the target movement position of the moving part to the current position or immediately sets the operating speed of the moving part to zero. However, this means places an excessive burden on the drive mechanism, which may cause a breakdown. In addition, the deceleration of the moving part becomes extremely large, which may cause adverse effects.

Therefore, the problem to be solved in this specification is to provide a speed command value calculation device that can suppress the actual occurrence of a fault by responding to an abnormal state in which a fault is expected to occur due to the moving operation of a movable part.

This specification discloses a speed command value calculation device that includes an original speed value calculation unit that calculates an original speed value for each predetermined calculation cycle based on the movement operation required of the movable part, a command value calculation unit that calculates a speed command value for each calculation cycle to be commanded to a drive mechanism that drives the movable part by performing moving average processing on the original speed value for each calculation cycle using a normal averaging parameter, an abnormality determination unit that determines an abnormal state in which a fault is expected to occur due to the movable part performing the movement operation, and an abnormality response unit that controls the original speed value calculation unit to set the original speed value to zero when the abnormality response unit determines that an abnormality response unit is detected, and controls the command value calculation unit to calculate the command value using an abnormal averaging parameter smaller than the normal averaging parameter.

This specification also discloses a speed command value calculation device that includes an original speed value calculation unit that calculates an original speed value for each predetermined calculation cycle based on the movement operation required of the movable part, a command value calculation unit that calculates a speed command value for each calculation cycle to be commanded to a drive mechanism that drives the movable part by performing a moving average process on the original speed value for each calculation cycle using a normal averaging parameter, an abnormality determination unit that determines an abnormal state in which the movable part is expected to collide with an obstacle or cross a boundary of a movable range as a result of the movement operation, and an abnormality response unit that controls the original speed value calculation unit to set the original speed value to zero when the abnormality is determined, and determines an abnormality averaging parameter smaller than the normal averaging parameter based on the distance between the current position of the movable part and the obstacle or the boundary, and controls the command value calculation unit to calculate the command value using the abnormality averaging parameter.

In the speed command value calculation device disclosed in this specification, when an abnormal state is determined, the abnormality response unit controls the original speed value to zero and calculates a command value using an abnormal averaging parameter smaller than the normal averaging parameter. As a result, the original speed value from the present onwards is zero, so the moving part immediately starts decelerating. Furthermore, because a small abnormal averaging parameter is used from the present onwards, the moving part comes to a stop at a greater deceleration than normal. As a result, the moving part can be stopped with a smaller braking distance than normal in an emergency stop operation when an abnormal state occurs, and the actual occurrence of an obstacle is suppressed.

1 is a perspective view showing a schematic configuration of a work robot to which a speed command value calculation device according to a first embodiment is applied; FIG. 2 is a block diagram showing a functional configuration of a speed command value calculation device. FIG. 4 is a diagram for explaining a buffer included in the command value calculation unit; FIG. 4 is an operational flow diagram illustrating the overall operation of the speed command value calculation device. FIG. 11 is an operational flow diagram illustrating an abnormality response operation of the speed command value calculation device. 4 is a diagram showing an original speed value and a command value in a normal state calculated by a speed command value calculation device; FIG. 11 is a table showing a method for calculating a command value by a moving average process in a command value calculation unit. FIG. 11 is a diagram showing an original speed value and a command value in an abnormal state calculated by a speed command value calculation device; FIG. FIG. 13 is a table showing a method for calculating a command value in an abnormal state by a command value calculation unit using moving average processing. 1 is a diagram showing an original speed value and a command value in an abnormal state calculated by a speed command value calculation device of the prior art; FIG. 13 is a tabular diagram showing a method for calculating a command value in an abnormal state by moving average processing in a command value calculation unit of the prior art. FIG. 11 is a block diagram showing a functional configuration of a speed command value calculation device according to a second embodiment. FIG. 11 is a diagram illustrating an original speed value and a command value in an abnormal state in the second embodiment. FIG. 11 is a table showing a method for calculating a command value in an abnormal state in the second embodiment.

1. Example of the configuration of the working robot 9 First, an example of the configuration of the working robot 9 to which the speed command value calculation device 1 of the first embodiment is applied will be described with reference to Figures 1 and 2. The working robot 9 is composed of a base 91, a movable arm 92 (movable part), an end effector 93, a drive mechanism 94, and a work command unit (not shown). The movable arm 92 is provided so as to be movable relative to the base 91. The movable arm 92 is composed of multiple members, and has multiple joints and multiple movable axes. The end effector 93 is provided at the tip of the movable arm 92, and carries out a commanded task.

The drive mechanism 94 drives the movement of the movable arm 92. Multiple sets of drive mechanisms 94 are provided corresponding to the number of movable axes. To simplify the following explanation, one set of drive mechanisms 94 corresponding to one movable axis will be described in detail. As shown in FIG. 2, the drive mechanism 94 is composed of a drive control unit 95, a drive device 96, and a brake device 97.

The drive device 96 causes the movable arm 92 to move. The drive device 96 is configured using a motor with good controllability, such as a servo motor or a pulse motor. Without being limited to this, the drive device 96 may be a device that drives the movable arm 92 using a hydraulic mechanism, a pneumatic mechanism, or the like.

The braking device 97 brakes the movement of the movable arm 92. The braking device 97 is configured, for example, using an electromagnetic brake that generates a braking force by electromagnetic force. Without being limited thereto, the braking device 97 may be configured, for example, using a friction brake that generates a braking force by the friction force of a friction plate.

The drive control unit 95 receives the speed command value CV, converts this command value CV into a physical quantity suitable for the drive device 96 and the brake device 97, and issues a command to the drive device 96 and the brake device 97. Suitable physical quantities correspond to the current value of the current flowing through the motor or electromagnetic brake, the pressure value of the hydraulic or pneumatic mechanism, the amount of movement of the friction plate in the friction brake, etc.

The work command unit controls the execution of instructed work based on work instructions from the operator and a preset work menu. The work command unit manages the movement motion required of the movable arm 92 and the work motion required of the end effector 93 when performing work. The movement motion required of the movable arm 92 is expressed by the movement direction and movement distance from the current position of the movable arm 92, or the final target position to which the movable arm 92 is to be moved, etc.

The speed, acceleration, and deceleration of the movable arm 92 when it moves have two constraints, and are regulated by the smaller of the two constraints. The first constraint is the performance limit of the drive mechanism 94. That is, the drive from the drive device 96 is constrained by the maximum speed and maximum acceleration that can occur during the movement of the movable arm 92. In addition, the braking from the braking device 97 is constrained by the maximum deceleration that can occur during the movement of the movable arm 92.

The second constraint condition is a constraint on the permissible speed, permissible acceleration, and permissible deceleration determined according to the movement operation of the movable arm 92. The second constraint condition is set so that no adverse effects occur when the movable arm 92 performs a movement operation. For example, if the movable arm 92 accelerates or decelerates excessively while the end effector 93 is gripping a part, the adverse effect of the part falling from the end effector 93 may occur. In addition, when the end effector 93 is moving while performing work such as painting, if the movable arm 92 moves at an excessive speed, the adverse effect of a decrease in work quality may occur. Taking such adverse effects into consideration, the permissible speed, permissible acceleration, and permissible deceleration are set appropriately.

2. Configuration of the Speed Command Value Calculation Device 1 of the First Embodiment Next, the configuration of the speed command value calculation device 1 of the first embodiment will be described with reference to Figs. 2, 3, and 6. The speed command value calculation device 1 calculates a speed command value CV based on a request command from a work command unit of the work robot 9, and outputs it to the drive control unit 95. In the first embodiment, the request command from the work command unit is represented by a movement distance L required for the movement operation of the movable arm 92. The speed command value calculation device 1 is configured using a computer device having an input unit and an output unit. As shown in Fig. 2, the speed command value calculation device 1 is configured with four control function units, namely, an original speed value calculation unit 2, a command value calculation unit 3, an abnormality determination unit 4, and an abnormality response unit 5.

The original speed value calculation unit 2 calculates the original speed value V0 for each predetermined calculation period TS based on the moving distance L required of the movable arm 92. The original speed value calculation unit 2 calculates a virtual constant original speed value V0 when the movable arm 92 moves. In other words, the movable arm 92 does not necessarily move at a constant original speed value V0. The constant original speed value V0 can be rephrased as a square wave in which a non-zero original speed value V0 continues for a certain period of time. Furthermore, the original speed value calculation unit 2 sets the maximum speed Vmax that can be generated by the drive device 96 (satisfies the first constraint) and does not exceed the allowable speed of the second constraint as the constant original speed value V0.

In the following description, it is assumed that the moving distance L is 200 mm, the cycle time ts of the calculation cycle TS is 1 ms, and the maximum value Vmax is 10 mm/ms. In an actual work robot 9, operating conditions and the like different from those assumed above are used. In the above assumption, the maximum moving distance L0 that the movable arm 92 can move per calculation cycle TS is calculated as 10 mm using the following formula.
Movement distance L0 = Vmax · ts = 10 × 1 [mm]

The minimum number of periods n required for the movable arm 92 to move the movement distance L is calculated by the following formula, and is 20 [periods].
Number of periods n = L/L0 = 200÷10 [periods]

Based on the above calculation results, the original speed value calculation unit 2 outputs the original speed value V0 = 10 [mm/ms] to the command value calculation unit 3 over a calculation period TS of 20 periods. In addition, the original speed value calculation unit 2 outputs the original speed value V0 = 0 to the command value calculation unit 3 during the calculation periods TS before and after the 20 periods excluding the original speed value V0. The original speed value V0 output by the original speed value calculation unit 2 can be changed by the abnormality response unit 5.

The command value calculation unit 3 calculates the speed command value CV for each calculation cycle TS to be instructed to the drive control unit 95 by performing moving average processing on the original speed value V0 for each calculation cycle TS using the normal averaging parameter NN. Here, the normal averaging parameter NN is set so that the acceleration of the movable arm 92 is equal to or less than the maximum acceleration of the first constraint condition, and does not exceed the allowable acceleration of the second constraint condition.

For example, if the maximum acceleration that satisfies the condition is 1 (units will be omitted hereafter), the stationary movable arm 92 can accelerate by 1 for each calculation cycle TS, and will reach the maximum value Vmax after 10 cycles. Therefore, the normal averaging parameter NN is set to 10, which corresponds to 10 cycles. If the normal averaging parameter NN is less than 10, the acceleration will exceed 1, and at least one of the first and second constraint conditions will not be satisfied. Furthermore, if the normal averaging parameter NN exceeds 10, the acceleration will be less than 1. This reduces the acceleration performance of the movable arm 92, making the movement operation redundantly long.

The command value calculation unit 3 is configured using a dedicated calculation element, for example, called a DSP (Digital Signal Processor). This allows the command value calculation unit 3 to perform high-speed calculation processing. The command value calculation unit 3 is configured from a buffer 6, an original speed value transfer unit 31, and an average calculation unit 32.

As shown in FIG. 3, the buffer 6 has ten first sample areas A1 to A10, which are equal to the normal averaging parameter NN. The buffer 6 stores ten original speed values V0 arranged in chronological order in the first sample area A1 to A10. The first sample area A1 stores the most recent original speed value V0, and the tenth sample area A10 stores the oldest original speed value V0. Each of the original speed values V0 stored in the buffer 6 can be changed by the abnormality response unit 5.

The original speed value moving unit 31 performs a moving process to move the original speed value V0 in the buffer 6. More specifically, the original speed value moving unit 31 moves the original speed values V0 stored in each of the first sample area A1 to the ninth sample area A9 in sequence to the older second sample area A2 to the tenth sample area A10 in response to an update of the calculation cycle TS (see the arrows in Figure 3). At this time, the original speed value V0 stored in the tenth sample area A10 becomes unnecessary. Furthermore, the original speed value moving unit 31 stores the current original speed value V0 in the newest first sample area A1.

The average calculation unit 32 calculates the command value CV by adding up the memory contents (10 original speed values V0) stored in the first sample area A1 to the tenth sample area A10 during each calculation period TS and dividing the sum by the normal averaging parameter NN (=10). The average calculation unit 32 further outputs the calculated command value CV to the drive control unit 95. The function of the average calculation unit 32 to add up the 10 memory contents cannot be changed. On the other hand, the normal averaging parameter NN can be changed by the abnormality response unit 5.

The abnormality determination unit 4 determines an abnormal state in which an obstacle is expected to occur due to the movement of the movable arm 92. The obstacle may be, but is not limited to, the movable arm 92 colliding with an obstacle, the movable arm 92 crossing the boundary of the set movable range, or the end effector 93 moving into an inoperable area where work cannot be performed.

The abnormality determination unit 4 stores information about the above-mentioned obstacles, movable range, and inoperable area in advance. In addition, information about the movement of the movable arm 92 is input to the abnormality determination unit 4 in real time. Therefore, the abnormality determination unit 4 can predict the occurrence of an obstacle. When the abnormality determination unit 4 determines that an abnormal state exists in a certain calculation cycle TS, it outputs an abnormality determination signal AB to the abnormality response unit 5 during that calculation cycle TS.

The abnormality determination unit 4 may determine an abnormal state based on a detection signal from a separately provided abnormality detection unit. For example, a monitoring camera provided near the end effector 93 of the movable arm 92 may be configured to detect an obstacle ahead of the movement of the movable arm 92 and transmit the detection signal to the abnormality determination unit 4. This makes it possible to respond to obstacles that may move, such as the operator's hand.

The abnormality response unit 5 is normally inactive and starts operating when it receives an abnormality determination signal AB. When an abnormality is determined, the abnormality response unit 5 controls the original speed value calculation unit 2 to forcibly set the subsequent original speed value V0 to 0. The abnormality response unit 5 also controls the command value calculation unit 3 to calculate the command value CV using an abnormality averaging parameter NA that is smaller than the normal averaging parameter NN.

Here, the abnormality response unit 5 multiplies the normal averaging parameter NN by a preset reduction rate R to obtain the abnormal averaging parameter NA. The reduction rate R is set to a minimum value that causes the deceleration of the movable arm 92 to be equal to or less than the maximum deceleration that can be generated by the braking device 97 (satisfying the first constraint) and does not exceed the allowable deceleration of the second constraint.

For example, if the maximum deceleration that satisfies the condition is 2, the movable arm 92 that moves at the maximum value Vmax (=10) can be decelerated by 2 for each calculation cycle TS and stopped after 5 cycles. Therefore, the reduction rate R is calculated as the ratio between the 5 cycles required for deceleration and the 10 cycles required for acceleration described above, and is set to 0.5 (R=0.5). According to this, the averaging parameter NA during abnormality is calculated by the following formula, which is 5.
Abnormal averaging parameter NA = NN・R = 10×0.5 [pieces]

If the reduction rate R is less than the minimum value of 0.5, the abnormality averaging parameter NA will be less than 5 and the deceleration will exceed 2, so at least one of the first and second constraints will not be satisfied. Also, if the reduction rate R exceeds the minimum value of 0.5, the abnormality averaging parameter NA will exceed 5 and the deceleration will be less than 2. This will reduce the deceleration performance of the movable arm 92 and make the braking distance required to stop it unnecessarily long.

The abnormality response unit 5 issues the calculated abnormality averaging parameter NA (=5) to the average calculation unit 32, changing the normal time averaging parameter NN to the abnormal time averaging parameter NA. In addition, when the abnormality response unit 5 receives the abnormality determination signal AB, it performs a predetermined abnormality set operation on the buffer 6. Details of the abnormality set operation will be explained in the abnormality response operation section described later.

3. Normal operation of the speed command value calculation device 1 Next, the normal operation of the speed command value calculation device 1 will be described with reference to Figs. 4, 6, and 7. In Fig. 6 (similar to Figs. 8 and 10), the horizontal axis represents the calculation period TS, and the vertical axis represents the speed. The dashed line graph indicates the original speed value V0 (actually a discrete amount for each calculation period TS) output by the original speed value calculation unit 2, and the black circle and solid line graph indicate the command value CV output by the command value calculation unit 3 and the change state connecting the command values CV. In Fig. 7 (similar to Figs. 9 and 11), the original speed value V0 and the command value CV stored in the first sample area A1 to the tenth sample area A10 of the buffer 6 in each calculation period TS are shown in the form of a list.

In step S1 of the operation flow shown in FIG. 4, the speed command value calculation device 1 performs initial settings at startup. That is, the speed command value calculation device 1 initializes the normal averaging parameter NN=10, the reduction rate R=0.5, and the original speed value V0=0. Note that these amounts may be saved as default values, and the initial settings in step S1 may be omitted.

In the next step S2, the original speed value calculation unit 2 determines whether or not there is a request for a movement operation. If there is no request, the original speed value calculation unit 2 advances the execution of the operation flow to step S4. If there is a request, the original speed value calculation unit 2 advances the execution of the operation flow to step S3 and calculates the original speed value V0. In the following explanation, we will assume that a request for a movement operation with the aforementioned movement distance L = 200 has occurred in the first calculation cycle TS1.

In step S3 of the first calculation cycle TS1, the original speed value calculation unit 2 can calculate the original speed value V0=10 for 20 cycles and the subsequent original speed value V0=0 at once. As shown in FIG. 6, the original speed value calculation unit 2 outputs the calculated original speed value V0 in the first calculation cycle TS1 and the subsequent calculation cycles TS. After this, the operation flow merges with step S4.

In step S4, the original speed value moving unit 31 performs the movement process in the buffer 6 described above. Before the first calculation cycle TS1, when there is no request for a movement operation, the original speed value V0 is 0, and the state in which 0 is stored in each of the first sample area A1 to the tenth sample area A10 continues. In the first calculation cycle TS1, the original speed value moving unit 31 stores the original speed value V0 = 10 input from the original speed value calculation unit 2 in the first sample area A1. At this time, the original speed value V0 stored in the second sample area A2 to the tenth sample area A10 is 0. This state is shown in the first calculation cycle TS1 row in Figure 7.

In the next step S5, the average calculation unit 32 calculates a command value CV=1 by dividing the sum of the 10 original speed values V0 stored in the first sample area A1 to the tenth sample area A10 by the normal averaging parameter NN (=10). In the next step S6, the average calculation unit 32 outputs the calculated command value CV=1 to the drive control unit 95.

In the next step S7, the speed command value calculation device 1 determines whether the output command value CV is 0 or not. Here, the command value CV being 0 means that the movement operation of the movable arm 92 has ended. In the first calculation cycle TS1, the command value CV is not 0, so execution of the operation flow proceeds to step S8. In step S8, the abnormality determination unit 4 determines whether an abnormal state exists. In many cases, it is not an abnormal state, and the operation flow is returned to step S4, and the calculation cycle TS is updated.

In step S4 of the second calculation cycle TS2, the original speed value moving unit 31 moves the original speed value V0=10 in the first sample area A1 to the second sample area A2, and stores the original speed value V0=10 input from the original speed value calculation unit 2 in the first sample area A1. At this time, the original speed values V0 stored in the third sample area A3 to the tenth sample area A10 are 0. This state is shown in the second calculation cycle TS2 row in Figure 7.

In the next step S5, the average calculation unit 32 calculates a command value CV=2 by dividing the sum of the 10 original speed values V0 stored in the first sample area A1 to the tenth sample area A10 by the normal averaging parameter NN (=10). In the next step S6, the average calculation unit 32 outputs the calculated command value CV=2 to the drive control unit 95. Thereafter, the operation flow returns to step S4 via steps S7 and S8, and the calculation cycle TS is updated.

When the repeat loop made up of steps S4 to S8 has been repeated 10 times, the tenth calculation cycle TS10 is reached. During this time, the command value CV increases by 1 for each calculation cycle TS. In the tenth calculation cycle TS10, the original speed value V0 = 10 is stored in each of the first sample area A1 to the tenth sample area A10. The average calculation unit 32 calculates the command value CV = 10 in step S5, and outputs the command value CV = 10 to the drive control unit 95 in step S6. Thereafter, the same state is maintained until the twentieth calculation cycle TS20.

In step S4 of the 21st calculation cycle TS21, the original speed value transfer unit 31 stores the original speed value V0 = 0 input from the original speed value calculation unit 2 in the first sample area A1. At this time, the original speed value V0 stored in the second sample area A2 to the tenth sample area A10 is 10. The average calculation unit 32 calculates a command value CV = 9 in step S5, and outputs the command value CV = 9 to the drive control unit 95 in step S6. From this point onwards, up to the 30th calculation cycle TS30, the command value CV decreases by 1 for each calculation cycle TS.

In the 30th calculation cycle TS30, since the command value CV is 0, execution of the operation flow is returned from step S7 to step S2, and the calculation cycle TS is updated. This ends the normal movement operation of the movable arm 92, and returns it to a state in which it can accept a request for the next movement operation. As shown in FIG. 6, the stopping operation from when the movable arm 92 starts to decelerate until it stops requires a stopping time of 10 cycles. In addition, the braking distance required for the stopping operation is calculated as the area of the command value CV from the 20th calculation cycle TS20 to the 30th calculation cycle TS30 in FIG. 6, and is 50.

If the requested travel distance L is longer than 200, the number of cycles for which the command value CV = 10 is maintained increases. In this case, the stopping time and braking distance required for the stopping operation remain the same as in the above case. If the requested travel distance L is shorter than 200, the number of cycles for which the command value CV = 10 is maintained decreases, or deceleration begins before the command value CV reaches 10.

4. Operation of the Speed Command Value Calculation Device 1 in Response to Abnormal Conditions Next, the operation of the speed command value calculation device 1 in response to abnormal conditions will be described with reference to Figs. 5, 8, and 9. If the abnormality determination unit 4 determines an abnormal state in step S8 of Fig. 4, the operation flow proceeds to step S9. In step S9, the speed command value calculation device 1 performs an abnormality response. Details of the operation flow in response to abnormal conditions are shown in Fig. 5. In the following, a case where an abnormality is determined in the 12th calculation cycle TS12 as exemplified in Figs. 8 and 9 will be described.

In step S11 of FIG. 5, the abnormality response unit 5 determines the abnormal averaging parameter NA (=5) as described above, and issues a command to the average calculation unit 32. The average calculation unit 32 changes the normal averaging parameter NN to the abnormal averaging parameter NA and continues operation. In the next step S12, the abnormality response unit 5 controls the original speed value calculation unit 2 to forcibly set the subsequent original speed value V0 to 0.

In the next step S13, the abnormality response unit 5 performs an abnormality set operation on the buffer 6. More specifically, the abnormality response unit 5 forcibly stores 0 in the sixth sample area A6 to the tenth sample area A10 in the calculation cycle TS after the twelfth calculation cycle TS12 in which the abnormal state was determined. The sixth sample area A6 to the tenth sample area A10 are determined as the five older sample areas obtained by subtracting the abnormality averaging parameter NA (=5) from the normal averaging parameter NN (=10). According to this, as shown in FIG. 9, the storage contents of the sixth sample area A6 to the tenth sample area A10 in the calculation cycle TS after the thirteenth calculation cycle TS13 are all 0. If the abnormality averaging parameter NA is eight, the abnormality response unit 5 forcibly stores 0 in the two older sample areas, the ninth sample area A9 and the tenth sample area A10 (see FIG. 14 described in the second embodiment).

This set operation essentially puts the sixth sample area A6 to the tenth sample area A10 into an unused state. This allows the average calculation unit 32 to correctly perform five moving average processing using the abnormal averaging parameter NA (=5) even if the function of adding up the stored contents of the ten sample areas cannot be changed.

It is also possible to configure the number of sample areas used by the original speed value shifting unit 31 and the average calculation unit 32 to be changeable. In this configuration, the anomaly response unit 5 can control the original speed value shifting unit 31 and the average calculation unit 32 to use only the first sample area A1 to the fifth sample area A5 on the newer side, which are equal to the abnormal averaging parameter NA.

Furthermore, in the 12th calculation cycle TS12 in which an abnormal state is determined, the abnormality response unit 5 stores the current command value CV = 10 in the first sample area A1 to the fifth sample area A5 on the newer side, which is equal to the abnormal averaging parameter NA. If an abnormal state is determined in the seventh calculation cycle TS7, the abnormality response unit 5 stores the current command value CV = 7 in the first sample area A1 to the fifth sample area A5. This setting operation is intended to avoid abrupt changes (unrealizable changes) in the command value CV that tend to occur when the averaging parameter is changed.

Then, the calculation cycle TS is updated, and the execution of the operation flow proceeds to step S14. The operation content in steps S14 to S17 is the same as that in steps S4 to S7 under normal conditions, except that the abnormal condition averaging parameter NA is used.

In step S14 of the thirteenth calculation cycle TS13, the original speed value moving unit 31 moves the command value CV=10 in the first sample area A1 to the fourth sample area A4 in sequence to the second sample area A2 to the fifth sample area A5. Furthermore, the original speed value moving unit 31 stores the original speed value V0=0 input from the original speed value calculation unit 2 in the first sample area A1. This state is shown in the thirteenth calculation cycle TS13 row in Figure 9.

As shown by arrow M1, the command value CV=10 for the 12th calculation cycle TS12 remains in the second sample area A2 to the fifth sample area A5 for the 13th calculation cycle TS13. As a result, the command value CV for the 13th calculation cycle TS13 does not change significantly from the command value CV for the 12th calculation cycle TS12. Therefore, abrupt changes in the command value CV are avoided.

In the next step S15, the average calculation unit 32 calculates a command value CV = 8. In the next step S16, the average calculation unit 32 outputs the calculated command value CV = 8 to the drive control unit 95. In the next step S17, since the command value CV is not 0, the operation flow returns to step S14, and the calculation cycle TS is updated. Thereafter, the repetitive loop made up of steps S14 to S17 is repeated. The command value CV is 10 in the 12th calculation cycle TS12, decreases by 2 each time the calculation cycle TS is updated, and becomes 0 in the 17th calculation cycle.

This completes the abnormality response, and the operation flow proceeds to step S18. In step S18, the abnormality response unit 5 returns the abnormality averaging parameter NA of the average calculation unit 32 to the normal averaging parameter NN, and cancels the abnormality set operation. After this, execution of the operation flow returns to step S2 via step S9 in FIG. 4, and the state returns to one in which a request for the next movement operation can be accepted.

When dealing with an abnormality, the emergency stop operation from when the movable arm 92 starts to decelerate until it stops requires a stopping time of five cycles. In addition, the braking distance required for the emergency stop operation is calculated as the area of the command value CV from the 12th calculation cycle TS12 to the 17th calculation cycle TS17 in Figure 8, and is 25.

5. Comparison with Prior Art Speed Command Value Calculation Device Next, the configuration and operation of the prior art speed command value calculation device will be described. The prior art speed command value calculation device includes the same original speed value calculation unit 2, command value calculation unit 3, and abnormality determination unit 4 as in the embodiment, and differs only in the function of the abnormality response unit. When an abnormal state is determined, the abnormality response unit of the prior art forcibly sets the subsequent original speed value V0 to 0 as in the embodiment, but does not change the normal averaging parameter NN or perform an abnormality setting operation of the buffer 6.

Therefore, in the speed command value calculation device of the prior art, the emergency stop operation of the movable arm 92 requires 10 cycles, the same as in normal times. As a result, as shown in Figures 10 and 11, if an abnormal state is determined in the 12th calculation cycle TS12, the stopping of the movable arm 92 is in the 22nd calculation cycle TS22. For this reason, the speed command value calculation device of the prior art requires the same braking distance (=50) as in normal times, even for an emergency stop operation to deal with an abnormality. In contrast, in the speed command value calculation device 1 of the embodiment, the stopping time (=5 cycles) and braking distance (=25) for the emergency stop operation are half.

In the speed command value calculation device 1 of the first embodiment, when an abnormal state is determined, the abnormality response unit 5 controls the original speed value V0 to zero and calculates the command value CV using an abnormality averaging parameter NA smaller than the normal averaging parameter NN. According to this, since the original speed value V0 from the present onwards becomes zero, the movable arm 92 (movable part) immediately starts decelerating. Furthermore, since a small abnormality averaging parameter NA is used from the present onwards, the movable arm 92 reaches a stop with a greater deceleration than normal. As a result, the movable arm 92 can be stopped with a smaller braking distance than normal in an emergency stop operation when an abnormal state occurs, and the actual occurrence of an obstacle is suppressed.

In addition, since the averaging parameter NA during an abnormality is set within the range permitted by the first constraint, excessive strain is not placed on the drive device 96 and the brake device 97, and the cause of failure can be reduced. Furthermore, since the averaging parameter NA during an abnormality is set within the range permitted by the second constraint, no adverse effects are caused to the movement operation of the movable arm 92 in response to an abnormality.

6. Speed command value calculation device 1A of the second embodiment
Next, the configuration and operation of a speed command value calculation device 1A of the second embodiment will be described with reference to Figures 12 to 14, focusing mainly on the differences from the first embodiment. The second embodiment differs from the first embodiment in that the abnormality determination unit 4 has an abnormality detection unit 98, and the operation of the abnormality response unit 5 differs from the first embodiment.

The abnormality detection unit 98 detects the current state of the movable arm 92, which is effective for determining an abnormal state. The abnormality detection unit 98 is composed of, for example, a surveillance camera or a proximity sensor provided near the end effector 93 of the movable arm 92, and detects obstacles ahead of the movement of the movable arm 92. The abnormality detection unit 98 outputs a signal regarding the presence or absence of an obstacle and a signal regarding the distance DX between the current position of the movable arm 92 and the obstacle to the abnormality determination unit 4.

The abnormality determination unit 4 determines an abnormal state based on the signal received from the abnormality detection unit 98, and calculates the above-mentioned separation distance DX. Examples of methods for calculating the separation distance DX include image processing of image data acquired by a surveillance camera and distance conversion of the output signal of a proximity sensor. The abnormality determination unit 4 outputs a signal representing the separation distance DX to the abnormality response unit 5, in addition to the abnormality determination signal AB similar to that of the first embodiment.

As in the first embodiment, the abnormality response unit 5 starts operating when it receives the abnormality determination signal AB. However, unlike the first embodiment, the reduction rate R is a variable amount that is not preset. The abnormality response unit 5 calculates the reduction rate R by a calculation using the separation distance DX. Furthermore, the abnormality response unit 5 multiplies the calculated reduction rate R by the normal averaging parameter NN to calculate the abnormal averaging parameter NB. In the second embodiment, the value of the reduction rate R may exceed 1 or may be smaller than the minimum value of 0.5 described in the first embodiment.

To explain the calculation method in detail, the abnormality response unit 5 first calculates the braking distance from the current position required for the movable arm 92 to stop, assuming that the command value CV calculated using the normal averaging parameter NN is followed. The braking distance when the movable arm 92 is moving at the maximum value Vmax (=10) is 50 as described in the first embodiment. When the movable arm 92 is moving at a speed slower than the maximum value Vmax, the braking distance will be less than 50, but it can be calculated. Next, the abnormality response unit 5 calculates the reduction rate R using the following equation.
Decrease rate R = DX/DS

Furthermore, the abnormality response unit 5 calculates the abnormality-time averaging parameter NB using the following formula: If the abnormality-time averaging parameter NB has a fraction less than 1, the fraction is rounded down to obtain an integer abnormality-time averaging parameter NB.
Abnormal averaging parameter NB = NN・R
In the following, the cases will be explained according to 1) to 3) based on the magnitude of the abnormal-state averaging parameter NB thus obtained.

1) When the abnormal averaging parameter NB is greater than the normal averaging parameter NN (NB > NN = 10)
In this case, the decrease rate R exceeds 1, and the braking distance calculated from the assumption is smaller than the separation distance DX. In other words, even if the movable arm 92 moves according to the command value CV calculated using the normal averaging parameter NN, it will not collide with the obstacle. Therefore, the abnormality response unit 5 controls the original speed value calculation unit 2 so that the subsequent original speed value V0 is forcibly set to 0, and does not control the command value calculation unit 3. The command value calculation unit 3 continues the moving average process using the normal averaging parameter NN.

2) When the abnormal averaging parameter NB is equal to or smaller than the normal averaging parameter NN and satisfies the first and second constraint conditions described in the first embodiment (NN=10≧NB≧5).
In this case, the decrease rate R is generally in the range of 0.5 or more and less than 1. In other words, the movable arm 92 may collide with an obstacle if the command value CV calculated using the normal averaging parameter NN is followed. Nevertheless, the command value calculation unit 3 uses the abnormal averaging parameter NB, so that the movable arm 92 can avoid the collision. Therefore, the abnormality response unit 5 controls the original speed value calculation unit 2 so as to forcibly set the subsequent original speed value V0 to 0, and also commands the average calculation unit 32 to set the abnormal averaging parameter NB, and performs a predetermined abnormality set operation on the buffer 6.

For example, as in the first embodiment, if an abnormal state is determined in the 12th calculation cycle TS12, and the separation distance DX = 42 and the braking distance = 50, the reduction rate R = 0.84 and the averaging parameter NB during abnormality is 8. The operation of the speed command value calculation device 1A in this case is shown in Figures 13 and 14.

As shown in FIG. 14, the contents stored in the ninth sample area A9 and the tenth sample area A10 in the calculation cycles TS from the thirteenth calculation cycle TS13 onwards are all 0. Also, as shown by the arrow M2, the command value CV=10 in the twelfth calculation cycle TS12 remains in the second sample area A2 to the eighth sample area A8 in the thirteenth calculation cycle TS13. Therefore, the command value CV in the thirteenth calculation cycle TS13 does not change significantly from the command value CV in the twelfth calculation cycle TS12. Furthermore, the command value CV in the thirteenth calculation cycle TS13 is 8.75, which is a larger deceleration than normal. The command value CV is 10 in the twelfth calculation cycle TS12, and decreases by 1.25 each time the calculation cycle TS is updated, to become 0 in the twentieth calculation cycle.

The braking distance required for the movable arm 92 to stop after it starts to decelerate is calculated as the area of the command value CV from the 12th calculation cycle TS12 to the 20th calculation cycle TS20 in FIG. 13, and is 40. In other words, the movable arm 92 can stop slightly before the obstacle with a braking distance (=40) that is slightly smaller than the separation distance DX (=42), thereby avoiding collision with the obstacle. In addition, because the deceleration is not made larger than necessary, it is possible to realize a gentle emergency stop operation that does not place a large burden on the movable arm 92 or the brake device 97, compared to the first embodiment in which an emergency stop operation is always performed at the maximum allowable deceleration.

3) When the abnormal averaging parameter N B does not satisfy the first and second constraint conditions described in the first embodiment (5>N B )
In this case, the reduction rate R is smaller than 0.5. In other words, the movable arm 92 cannot avoid colliding with the obstacle as long as it follows the command value CV calculated using the abnormality averaging parameter NB that satisfies the first and second constraint conditions. The abnormality response unit 5 controls the original speed value calculation unit 2 so as to forcibly set the subsequent original speed value V0 to 0, and commands the average calculation unit 32 to set the minimum abnormality averaging parameter NB (=5) that satisfies the first and second constraint conditions, and performs a predetermined abnormality set operation on the buffer 6.

According to this abnormality response, the movable arm 92 stops at the braking distance DS = 25 as described in the first embodiment. Therefore, the speed of the movable arm 92 at the moment of collision with the obstacle is minimized within the range permitted by the first and second constraints, and the shock at the time of collision is mitigated to a minimum.

In the speed command value calculation device 1A of the second embodiment, as in the first embodiment, the movable arm 92 can be stopped with a braking distance smaller than normal during an emergency stop operation when an abnormal state occurs, and the actual occurrence of an obstacle is suppressed. In addition, the abnormality averaging parameter NB can be variably and appropriately set according to the size of the separation distance DX, so that a gentle emergency stop operation that does not place a large burden on the movable arm 92 or the brake device 97 can be realized.

7. Application and Modification of the Embodiments The period time ts (=1 ms) of the calculation period TS exemplified in the first embodiment is set to be smaller in the actual machine, and the normal averaging parameter NN (=10) is set to be larger in the actual machine, so that fine movement control is performed. In the first embodiment, the command value calculation unit 3 is configured using a dedicated calculation element, but may be configured using the memory and program of the computer device that configures the speed command value calculation device 1. Furthermore, the second embodiment can be applied to a configuration in which the movable range of the movable arm 92 is divided by physical boundaries. In this configuration, the abnormality detection unit 98 detects the physical boundary in front of the movement of the movable arm 92. The first and second embodiments can be applied and modified in various other ways.

The speed command value calculation device (1, 1A) of the first and second embodiments is not limited to the work robot 9, and can be used in various devices that calculate a speed command value using moving average processing.

1, 1A: Speed command value calculation device 2: Original speed value calculation section 3: Command value calculation section 31: Original speed value movement section 32: Average calculation section 4: Abnormality judgment section 5: Abnormality response section 6: Buffer 9: Work robot 91: Base section 92: Movable arm 93: End effector 94: Drive mechanism 95: Drive control section 96: Drive unit 97: Brake unit A1-A10: 1st to 10th sample areas L: Movement distance NN: Normal averaging parameter NA: Abnormal averaging parameter R: Decrease rate V0: Original speed value Vmax: Maximum value (of speed) CV: (of speed) command value TS: Calculation period

Claims (12)

an original speed value calculation unit that calculates an original speed value for each predetermined calculation period based on a moving operation required of the movable part;
a command value calculation unit that calculates a command value of a speed for each calculation cycle to be commanded to a drive mechanism that drives the movable part by performing a moving average process on the original speed value for each calculation cycle using a normal averaging parameter;
an abnormality determination unit that determines an abnormal state in which a fault is expected to occur due to the moving operation of the movable unit;
an abnormality response unit that controls the original speed value calculation unit to set the original speed value to zero when the abnormal state is determined, and controls the command value calculation unit to calculate the command value using an abnormal averaging parameter smaller than the normal averaging parameter;
A speed command value calculation device comprising: The speed command value calculation device according to claim 1, wherein the abnormality response unit obtains the abnormality average parameter by multiplying the normal average parameter by a preset reduction rate. The speed command value calculation device according to claim 2, wherein the reduction rate is set so that the deceleration of the movable part does not exceed an allowable deceleration determined according to the movement operation and is equal to or less than the maximum deceleration that can be generated by the drive mechanism. an original speed value calculation unit that calculates an original speed value for each predetermined calculation period based on a moving operation required of the movable part;
a command value calculation unit that calculates a command value of a speed for each calculation cycle to be commanded to a drive mechanism that drives the movable part by performing a moving average process on the original speed value for each calculation cycle using a normal averaging parameter;
an abnormality determination unit that determines an abnormal state in which the movable unit is expected to collide with an obstacle or go beyond a boundary of a movable range as a result of the moving operation of the movable unit;
an abnormality response unit that controls the original speed value calculation unit to set the original speed value to zero when the abnormal state is determined, and that determines an abnormality averaging parameter smaller than the normal averaging parameter based on a distance between a current position of the movable part and the obstacle or the boundary, and controls the command value calculation unit to calculate the command value using the abnormality averaging parameter;
A speed command value calculation device comprising: The speed command value calculation device according to claim 4, wherein the abnormality response unit calculates the braking distance from the current position required for the movable part to stop, assuming that the command value calculated using the normal averaging parameter is followed, and calculates the abnormality averaging parameter based on the separation distance and the braking distance. The speed command value calculation device according to any one of claims 1 to 5, wherein the normal averaging parameter is set so that the acceleration of the movable part does not exceed an allowable acceleration determined according to the movement operation and is equal to or less than the maximum acceleration that can be generated by the drive mechanism. The command value calculation unit is
a buffer for storing a plurality of said original speed values arranged in time series in a sample area arranged in order of newest to oldest, the number of which is equal to said normal averaging parameter;
an original speed value moving unit which moves the original speed values stored in each of the sample areas to the older sample area in sequence in response to the update of the calculation period, and stores the current original speed value in the newest sample area;
an average calculation unit that calculates the command value by adding up the original speed values stored in the respective sample areas in each of the calculation cycles and dividing the sum by the normal averaging parameter,
The speed command value calculation device according to any one of claims 1 to 6. The abnormality response unit is
In the calculation cycle in which the abnormal state is determined, the current command value is stored in the new side sample area of which the number is equal to the abnormal state averaging parameter, and the normal state averaging parameter of the average calculation unit is changed to the abnormal state averaging parameter; and
storing zero in the sample area on the older side of the number obtained by subtracting the abnormal state averaging parameter from the normal state averaging parameter in the calculation cycle after the abnormal state is determined;
The speed command value calculation device according to claim 7. The abnormality response unit is
In the calculation cycle in which the abnormal state is determined, the current command value is stored in the new side sample area of which the number is equal to the abnormal state averaging parameter, and the normal state averaging parameter of the average calculation unit is changed to the abnormal state averaging parameter; and
control the original speed value shifting unit and the average calculation unit to use only the new side sample areas of a number equal to the abnormal-time averaging parameter in the calculation cycle after the abnormal state is determined;
The speed command value calculation device according to claim 7. The speed command value calculation device according to any one of claims 1 to 9, wherein the original speed value calculation unit calculates a constant original speed value. The speed command value calculation device according to claim 10, wherein the original speed value calculation unit sets the maximum speed that can be generated by the drive mechanism to a constant original speed value that does not exceed an allowable speed determined according to the movement operation of the movable part. the movable part is a movable arm of a working robot,
The drive mechanism includes a drive device that causes the movable arm to perform the moving operation, and a braking device that brakes the moving operation of the movable arm.
The speed command value calculation device according to any one of claims 1 to 11.

Images