Robot drive device, robot drive method, and robot drive program

Abstract

To solve such a problem that, in a conventional robot drive device, it has been difficult to sufficiently stabilize both position control and torque control.SOLUTION: A robot drive device according to the present invention includes: a motor drive circuit that increases / decreases the drive current given to a motor attached to a joint of a robot according to a control command value and outputs a position current value corresponding to a rotation angle of the motor; a control command value generation unit that generates the control command value; and a correction processing unit that, in a period in synchronization with the periodic processing of the control command value generation unit, outputs a corrected position target value and corrected virtual spring constant obtained by correcting a position target value and a virtual spring constant to the control command value generation circuit. In the correction processing unit, a position control method that corrects the position target value is applied to low-frequency disturbances, and a virtual spring constant control method that corrects the virtual spring constant is applied to high-frequency disturbances in the disturbances given to the motor.SELECTED DRAWING: Figure 5

Description

[Technical field]

[0001] The present invention relates to a robot driving device, a robot driving method, and a robot driving program, and more particularly to a robot driving device, a robot driving method, and a robot driving program that control a robot using a virtual spring constant and a position target. [Background technology]

[0002] In a robot with an arm that makes contact with an object, when the arm makes contact with or collides with an object, the input torque change amount is large and the frequency of the change is high, which causes a problem that the control tends to become unstable. Therefore, Patent Document 1 discloses a technology for stabilizing such unstable control.

[0003] The robot described in Patent Document 1 is a robot comprising a main body, a plurality of moving mechanism units each including a moving mechanism for moving on a floor surface, and a plurality of legs each including one or more joint units and connecting the main body and each of the moving mechanism units, and the joint units include a drive shaft driven by impedance control. [Prior art documents] [Patent documents]

[0004] [Patent Document 1] Patent Publication No. 2022-099034 Summary of the Invention [Problem to be solved by the invention]

[0005] However, in the position control described in Patent Document 1, the virtual spring constant needs to be set relatively high to ensure the accuracy of the position control, and if the calculation accuracy of the current position is low, it is difficult to change the target position and vibrations are likely to occur. In addition, in the spring constant control, there is a problem that the value that the virtual spring constant can take has a lower limit, making it impossible to tolerate position deviation. In this way, the robot described in Patent Document 1 has a problem in that it is difficult to sufficiently stabilize the position control and torque control because there are restrictions on the use of the control parameters obtained to stabilize the position control and torque control.

[0006] The present invention has been made to solve such problems, and has an object to provide more stable position control in a driving section such as a robot arm. [Means for solving the problem]

[0007] One aspect of a robot driving device according to the present invention has a motor driving circuit that increases or decreases a driving current given to a motor attached to a joint of the robot in accordance with a control command value, and outputs a current position value corresponding to the rotational angle of the motor; a control command value generation unit that generates the control command value using at least the current position value, a position target value of the robot's joint, a virtual spring constant, a torque target value, and a speed target value; and a correction processing unit that outputs to the control command value generation unit a corrected position target value and a corrected virtual spring constant obtained by correcting the position target value and the virtual spring constant in a period synchronized with the periodic processing of the control command value generation unit, wherein the correction processing unit performs correction processing by applying a position control technique that applies a correction to the position target value for low-frequency disturbance components of disturbances applied to the motor, and applying a virtual spring constant control technique that applies a correction to the virtual spring constant for high-frequency disturbance components.

[0008] One aspect of a robot driving method according to the present invention is a robot driving method for a robot driving device having a motor driving circuit that increases or decreases a driving current given to a motor attached to a joint of the robot in accordance with a control command value, and outputs a current position value corresponding to the rotational angle of the motor, a control command value generation unit that generates the control command value using at least the current position value, a position target value of the robot's joint, a virtual spring constant, a torque target value, and a speed target value, and a correction processing unit that outputs a corrected position target value and a corrected virtual spring constant, which are obtained by correcting the position target value and the virtual spring constant, to the control command value generation unit in a period synchronized with the periodic processing of the control command value generation unit, wherein the correction processing unit performs correction processing using a position control technique that applies a correction to the position target value for low-frequency disturbance components of disturbances applied to the motor, and a virtual spring constant control technique that applies a correction to the virtual spring constant for high-frequency disturbance components.

[0009] One aspect of a robot driving program according to the present invention includes a motor driving circuit that increases or decreases a driving current given to a motor attached to a joint of a robot in accordance with a control command value, and outputs a current position value corresponding to the rotational angle of the motor; a control command value generation unit that generates the control command value using at least the current position value, a position target value of the robot's joint, a virtual spring constant, a torque target value, and a speed target value; and a correction processing unit that outputs a corrected position target value and a corrected virtual spring constant, obtained by correcting the position target value and the virtual spring constant, to the control command value generation unit in a period synchronized with the periodic processing of the control command value generation unit, wherein in a robot driving device having a calculation unit that executes calculation processing in the correction processing unit, a position control technique is applied that adds a correction to the position target value for low-frequency disturbance components of disturbances given to the motor, and a virtual spring constant control technique is applied that adds a correction to the virtual spring constant for high-frequency disturbance components.

[0010] In the robot driving device, robot driving method, and robot driving program of the present invention, a correction is made to the position target value when the frequency of the disturbance applied to the motor is low, and a correction is made to the virtual spring constant when the frequency of the disturbance is high. Effect of the Invention

[0011] According to the present invention, the stability of the position control of the robot can be improved. [Brief description of the drawings]

[0012] [Figure 1] FIG. 1 is a block diagram of a robot driving device according to a first embodiment. [Diagram 2] 11 is a diagram illustrating a control logic when a position target value is changed. FIG. [Diagram 3] 11A and 11B are diagrams illustrating a control logic when a virtual spring constant is changed. [Figure 4] FIG. 2 is a block diagram of a correction processing unit according to the first embodiment. [Diagram 5] 4 is a flowchart illustrating an operation of the correction processing unit according to the first embodiment. [Figure 6] FIG. 11 is a block diagram of a correction processing unit according to a second embodiment. [Figure 7] 13 is a flowchart illustrating an operation of a correction processing unit according to the second embodiment. DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS

[0013] In order to clarify the description, the following description and drawings are omitted and simplified as appropriate. In addition, each element shown in the drawings as a functional block performing various processes can be configured in hardware with a CPU (Central Processing Unit), memory, and other circuits, and in software, can be realized by a program loaded into memory. Therefore, it is understood by those skilled in the art that these functional blocks can be realized in various forms by hardware alone, software alone, or a combination thereof, and are not limited to any one of them. In addition, the same elements are given the same symbols in each drawing, and duplicated explanations are omitted as necessary.

[0014] Moreover, the above-mentioned program includes a set of instructions (or software code) for making the computer perform one or more functions described in the embodiment when the program is read into the computer. The program may be stored in a non-transitory computer-readable medium or a tangible storage medium. By way of example and not limitation, the computer-readable medium or the tangible storage medium includes random-access memory (RAM), read-only memory (ROM), flash memory, solid-state drive (SSD) or other memory technology, CD-ROM, digital versatile disc (DVD), Blu-ray (registered trademark) disk or other optical disk storage, magnetic cassette, magnetic tape, magnetic disk storage or other magnetic storage device. The program may be transmitted on a transitory computer-readable medium or a communication medium. By way of example and not limitation, the transitory computer-readable medium or the communication medium includes electrical, optical, acoustic, or other forms of propagating signals.

[0015] First embodiment FIG. 1 shows a block diagram of a robot driving device 1 according to the first embodiment. In the example shown in FIG. 1, a robot is operated by driving a motor provided at a joint of an arm of the robot. As shown in FIG. 1, the robot driving device 1 according to the first embodiment has a correction processing unit 10, a control command value generating unit 11, and a motor driving circuit 12. In the robot driving device 1, a position target value Pos_ref, a virtual spring constant Kp_ref, a torque target value τref, a speed target value Vel_ref, and a control gain Kd_ref are provided from a higher-level system (not shown). In addition, in the robot driving device 1, there are cases where the control gain Kd_ref is not provided.

[0016] The motor drive circuit 12 increases or decreases the drive current I_od given to the motor attached to the joint of the robot according to the control command value, and outputs the current position value Pos_cur corresponding to the rotation angle of the motor. The control command value generation unit 11, the correction processing unit 10, generates a control command value using at least the current position value Pos_cur, the position target value Pos_ref of the joint of the robot, the virtual spring constant Kp_ref, the torque target value τref, and the velocity target value Vel_ref. The correction processing unit 10 outputs a corrected position target value Pos_ref_c and a corrected virtual spring constant Kp_ref_c obtained by correcting the position target value Pos_ref and the virtual spring constant Kp_ref in a period synchronized with the periodic processing of the control command value generation unit 11. In the first embodiment, the control command value generation unit 11 uses the corrected position target value Pos_ref_c and the corrected virtual spring constant Kp_ref_c output by the correction processing unit 10 as the position target value Pos_ref and the virtual spring constant Kp_ref.

[0017] Fig. 1 shows an example of the configuration of the control command value generating unit 11. Specifically, Fig. 1 shows an example in which the control command value generating unit 11 has a subtractor 21, a multiplier 22, a current speed calculating unit 23, a subtractor 24, a multiplier 25, an adder 26, and an adder 27.

[0018] The subtractor 21 subtracts the current position value Pos_cur from the corrected position target value Pos_ref_c to output a position deviation. The multiplier 22 multiplies the position deviation output by the subtractor 21 by the corrected virtual spring constant Kp_ref_c to output a value. The current speed calculation unit 23 differentiates the current position value Pos_cur to output a current speed value. The subtractor 24 subtracts the current speed value from the speed target value Vel_ref to output a speed deviation. The multiplier 25 multiplies the speed deviation output by the subtractor 24 by the control gain Kd_ref to output a value. The adder 26 adds the value output by the multiplier 25 to the torque target value τref to output a value. The adder 27 adds the value output by the adder 27 to the value output by the multiplier 22 to output a control command value to be given to the motor drive circuit 12.

[0019] In the robot driving device 1 according to the first embodiment, the correction processing unit 10 performs correction processing by applying a position control method for applying correction to the position target value Pos_ref for low-frequency disturbance components of disturbances applied to the motor, and applying a virtual spring constant control method for applying correction to the virtual spring constant Kp_ref for high-frequency disturbance components. Specifically, the correction processing unit 10 performs correction corresponding to the disturbance factor applied to the motor for at least one of the position target value Pos_ref and the virtual spring constant Kp_ref. In the following description, an example is described in which correction processing is switched using a parameter that indicates torque among parameters that vary due to disturbances applied to the motor, but it is possible to use a parameter that includes a component with a different frequency among parameters that can capture changes occurring in the motor due to disturbances for switching correction processing.

[0020] More specifically, in the robot driving device 1 according to the first embodiment, the frequency characteristics of the torque fluctuation are analyzed, and a first correction process using a position control method that corrects a position target value is performed for torque fluctuation on the low frequency side, and a second correction process using a virtual spring constant control method that corrects a virtual spring constant is performed for torque fluctuation on the high frequency side. Here, the control logic of the position control method and the virtual spring constant control method will be explained.

[0021] FIG. 2 shows a diagram for explaining the control logic when the position target value is changed (position control method). In FIG. 2, the virtual spring constant is assumed to be kept constant. In the example shown in FIG. 2, the estimated torque τcur reaches the torque target value τref when the difference between the current position value and the position target value Pos_ref, which is the target value of the position, reaches the position difference P1. Then, in the position control method, when the difference between the current position value and the position target value Pos_ref further increases, the position target value Pos_ref is corrected to the corrected position target value Pos_ref_c (the corrected target position in FIG. 2) so that the difference from the current position value becomes constant. In the case where the position control method is not applied, when the position difference between the current position value and the position target value Pos_ref increases beyond the position difference P1, the estimated torque becomes higher than the torque target value τref in order to bring the current position value closer to the position target value Pos_ref (the uncorrected torque in FIG. 2). On the other hand, when this position control method is applied, even if the position difference between the current position value and the target position value Pos_ref becomes larger than the position difference P1, the corrected estimated torque is maintained at the target torque value τref.

[0022] Next, FIG. 3 shows a diagram for explaining the control logic when the virtual spring constant is changed (virtual spring constant control method). In the example shown in FIG. 3, the position target value is assumed to be kept constant. In the example shown in FIG. 3, the estimated torque τcur reaches the torque target value τref when the difference between the current position value and the position target value Pos_ref, which is the target value of the position, reaches the position difference P1. In the virtual spring constant method, when the difference between the current position value and the position target value Pos_ref further increases, the virtual spring constant Kp is corrected so as to be gradually smaller than the virtual spring constant Kp_ref according to the magnitude of the position difference. In the case where the virtual spring constant control method is not applied, when the position difference between the current position value and the position target value Pos_ref increases beyond the position difference P1, the estimated torque becomes higher than the torque target value τref in order to bring the current position value closer to the position target value Pos_ref (torque without correction in FIG. 2). On the other hand, when this virtual spring constant control method is applied, even if the position difference between the current position value and the target position value Pos_ref becomes larger than the position difference P1, the corrected estimated torque is maintained at the target torque value τref.

[0023] As described in FIG. 2 and FIG. 3, whether the position control method or the virtual spring constant control method is used, it is possible to limit the estimated torque so that the torque target value τref is the upper limit. However, there is a difference between the two methods in that the position control method is more effective for torque control that fluctuates at low frequencies, and the virtual spring constant control method is more effective for torque control that fluctuates at high frequencies. Therefore, in the robot driving device 1 according to the first embodiment, the correction processing unit 10 performs a first correction process using the position control method that corrects the position target value for torque fluctuations on the low frequency side, and a second correction process using the virtual spring constant control method that corrects the virtual spring constant for torque fluctuations on the high frequency side. Therefore, the configuration and operation of the correction processing unit 10 will be described in more detail below.

[0024] The correction processing unit 10 performs a reference process, a first correction process, and a second correction process when performing the correction process. In the reference process, a disturbance index target value, which is a preset value and is a target value of a disturbance index that varies due to a disturbance applied to the motor, is referenced. In the following description, a torque value is used as the disturbance index, and the correction processing unit 10 references a torque target value τref as the target value of the disturbance index. In addition, the correction processing unit 10 uses an estimated value obtained by calculation using a current position value obtained from the motor as the torque of the motor. In the following description, this estimated value of the torque is referred to as an estimated torque. Note that, in the correction processing unit 10 described below, an example is described in which an estimated torque is calculated using a current position value Pos_cur output by the motor drive circuit 12 based on a rotation angle obtained in motor control, but a value obtained from parameters obtained using a torque sensor, a current sensor, or the like can also be used as the estimated torque described below. Note that since the torque sensor has a low response frequency (for example, 1 KHz or less), the torque value obtained from the torque sensor is suitable as a substitute value for the low-frequency side estimated torque τcur_L described below. Furthermore, since the current sensor has a high response frequency (eg, 200 KHz or less), the estimated torque calculated from the current value obtained from the current sensor is suitable as a substitute for the high frequency side estimated torque τcur_H described below.

[0025] In the first correction process, if a low-frequency disturbance component (e.g., low-frequency estimated torque τcur_L) having low-frequency characteristics among the components of the disturbance index (estimated torque) is greater than or equal to the disturbance index target value (e.g., torque target value τref), a value calculated based on a predetermined calculation formula is set to the corrected position target value Pos_ref_c, and if the low-frequency disturbance component is smaller than the disturbance index target value, a predetermined initial value (e.g., position target value Pos_ref) is set to the corrected position target value Pos_ref_c.

[0026] In the second correction process, if a high-frequency disturbance component (e.g., high-frequency estimated torque τcur_H) having high-frequency frequency characteristics among the components of the disturbance index (estimated torque) is greater than or equal to the disturbance index target value (e.g., torque target value τref), a value calculated based on a predetermined calculation formula is set as the corrected virtual spring constant Kp_ref_c, and if the low-frequency disturbance component is smaller than the disturbance index target value, a predetermined initial value is set as the corrected virtual spring constant Kp_ref_c.

[0027] A configuration example of the correction processing unit 10 that realizes the above operation will be described. FIG. 4 shows a block diagram of the correction processing unit 10 according to the first embodiment. As shown in FIG. 4, the correction processing unit 10 has a correction position calculation unit 31, a correction Kp calculation unit 32, delay circuits 33 and 34, and a filter processing unit 35. In the robot driving device 1 according to the first embodiment, the control command value generation unit 11 generates a control command value to be given to the motor driving circuit 12 at a predetermined period, so the correction processing unit 10 also performs correction processing at a period synchronized with the period of the control command value generation unit 11. In FIG. 4 and the following description, n is shown as a symbol indicating the value of the current period, and n-1 is shown as a symbol indicating the value of the previous period.

[0028] The corrected position calculation unit 31 performs a reference process and a first correction process. Specifically, in the reference process, the corrected position calculation unit 31 refers to a given torque target value τref. In addition, the corrected position calculation unit 31 compares the torque target value τref with the low-frequency side estimated torque τcur_L calculated in the filter processing unit 35, and calculates a corrected position target value Pos_ref_c, which is a correction value of the position target value Pos_ref, based on equation (1) if the low-frequency side estimated torque τcur_L is equal to or greater than the torque target value τref. Pos_ref_c[n]=(τcur_L-τref)*Kp_ref*Pos_ref_c[n-1]···(1) In addition, in the formula (1), Kp_ref is a virtual spring constant Kp_ref. On the other hand, when the low-frequency side estimated torque τcur_L is smaller than the torque target value τref, the corrected position calculation unit 31 calculates a corrected position target value Pos_ref_c, which is a correction value of the position target value Pos_ref, based on the formula (2). Pos_ref_c[n] = Pos_ref (2) That is, when the low-frequency estimated torque τcur_L is smaller than the torque target value τref, the corrected position calculation unit 31 returns the corrected position target value Pos_ref_c to the position target value Pos_ref or maintains it at that value.

[0029] The correction Kp calculation unit 32 performs a reference process and a second correction process. Specifically, in the reference process, the correction Kp calculation unit 32 refers to a given torque target value τref. In addition, the correction Kp calculation unit 32 compares the torque target value τref with the high frequency side estimated torque τcur_H calculated in the filter processing unit 35, and calculates a corrected virtual spring constant Kp_ref_c, which is a correction value of the virtual spring constant Kp_ref, based on the formula (3) when the high frequency side estimated torque τcur_H is equal to or greater than the torque target value τref. Kp_ref[n]=τref / (Pos_σ_H)···(3) In addition, Pos_σ_H in equation (3) is the position deviation on the high frequency side and is expressed by equation (4). Pos_σ_H=Pos_ref_c[n]-Pos_cur_L···(4) In the formula (4), Pos_cur_L is the high frequency side current position value Pos_cur_H. On the other hand, when the high frequency side estimated torque τcur_H is smaller than the torque target value τref, the correction Kp calculation unit 32 calculates a corrected virtual spring constant Kp_ref_c, which is a correction value of the virtual spring constant Kp_ref, based on the formula (5). Kp_ref_c[n] = Kp_ref (5) That is, when the high frequency side estimated torque τcur_H is smaller than the torque target value τref, the correction Kp calculation section 32 returns the corrected virtual spring constant Kp_ref_c to the virtual spring constant Kp_ref or maintains it at that value.

[0030] The delay circuit 33 holds the corrected position target value Pos_ref_c output by the corrected position calculation unit 31 until the next processing cycle. The delay circuit 34 holds the corrected virtual spring constant Kp_ref_c output by the correction Kp calculation unit 32 until the next processing cycle.

[0031] The filter processing unit 35 separates the position deviation of the joint or the torque deviation of the motor into high-frequency side and low-frequency side components. Specifically, the filter processing unit 35 acquires the current position value Pos_cur from the motor, and performs filtering on the acquired current position value Pos_cur to calculate a low-frequency side current position value Pos_cur_L and a high-frequency side current position value Pos_cur_H. Then, the filter processing unit 35 calculates a low-frequency side position deviation and a low-frequency side estimated torque τcur_L using the low-frequency side current position value Pos_cur_L. In addition, the filter processing unit 35 calculates a high-frequency side position deviation Pos_σ_H and a high-frequency side estimated torque τcur_H using the high-frequency side current position value Pos_cur_H.

[0032] To perform the above calculations, the filter processing unit 35 has a low-pass filter 41, a subtractor 42, a multiplier 43, a hyperfilter 44, a subtractor 45, and a multiplier 46. The low-pass filter 41 performs low-pass filtering on the current position value Pos_cur to output a low-frequency side current position value Pos_cur_L. The subtractor 42 subtracts the low-frequency side current position value Pos_cur_L from the corrected previous position target value Pos_ref_c[n-1] to output a low-frequency side position deviation. The multiplier 43 multiplies the corrected previous virtual spring constant value Kp_ref_c[n-1] by the low-frequency side position deviation output by the subtractor 42 to output a low-frequency side estimated torque τcur_L.

[0033] The hyperfilter 44 performs high-pass processing on the current position value Pos_cur and outputs the high frequency side current position value Pos_cur_H. The subtractor 45 subtracts the high frequency side current position value Pos_cur_H from the corrected position target value Pos_ref_c[n] and outputs the high frequency side position deviation Pos_σ_H. The multiplier 46 multiplies the previous corrected virtual spring constant value Kp_ref_c[n-1] by the high frequency side position deviation Pos_σ_H output by the subtractor 45 and outputs the high frequency side estimated torque τcur_H.

[0034] The order of performing the above processing will be specifically described. Fig. 5 shows a flowchart for explaining the operation of the correction processing unit 10 according to the first embodiment. The correction processing unit 10 repeats the processing shown in Fig. 5 at a cycle synchronized with the processing cycle of the control command value generating unit 11.

[0035] 5, when the correction processing unit 10 starts the process, first, it acquires the current position value Pos_cur (step S1). Then, the correction processing unit 10 calculates the low-frequency side current position value Pos_cur_L from the current position value Pos_cur in the low-pass filter 41 (step S2). Furthermore, the correction processing unit 10 calculates the high-frequency side current position value Pos_cur_H from the current position value Pos_cur in the hyper-filter 44 (step S3).

[0036] Next, the correction processing unit 10 calculates the low-frequency side estimated torque τcur_L using the corrected previous position target value Pos_ref_c[n-1], the low-frequency side current position value Pos_cur_L, and the corrected previous virtual spring constant value Kp_ref_c[n-1] in the filter processing unit 35. After that, the correction processing unit 10 calculates the corrected position target value Pos_ref_c[n] in the corrected position calculation unit 31 (step S5). In this step S5, the corrected position calculation unit 31 selects either one of the calculation formulas (1) and (2) based on the magnitude relationship between the low-frequency side estimated torque τcur_L and the torque target value τref to calculate the corrected position target value Pos_ref_c[n].

[0037] Next, the correction processing unit 10 calculates the high frequency side position deviation Pos_σ_H in the filter processing unit 35 using the corrected position target value Pos_ref_c[n] calculated in step S5 and the high frequency side position current value Pos_cur_H (step S6). The correction processing unit 10 also calculates the high frequency side estimated torque τcur_H in the filter processing unit 35 using the corrected virtual spring constant previous value Kp_ref_c[n-1] and the high frequency side position deviation Pos_σ_H (step S7). After that, the correction processing unit 10 calculates the corrected virtual spring constant Kp_ref_c[n] in the correction Kp calculation unit 32 (step S8). In this step S8, the correction Kp calculation unit 32 selects either one of the calculation formulas (3) and (5) based on the magnitude relationship between the high frequency side estimated torque τcur_H and the torque target value τref to calculate the corrected virtual spring constant Kp_ref_c[n].

[0038] As described above, in the robot driving device 1 according to the first embodiment, the correction processing unit 10 separates the torque fluctuation caused by the disturbance into a low-frequency component and a high-frequency component, and applies a position control method to the low-frequency component and a virtual spring constant control method to the high-frequency component. This makes it possible for the robot driving device 1 according to the first embodiment to eliminate the limitations of a control method that applies only one of the position control method and the virtual spring constant control method. Thus, the robot driving device 1 according to the first embodiment can achieve higher stability than a control method that applies only one of the position control method and the virtual spring constant control method.

[0039] Specifically, the robot driving device 1 according to the first embodiment can stably control the position and torque when an arm or the like driven by a motor collides with or comes into contact with an object. Also, the robot driving device 1 according to the first embodiment can keep the torque constant when the position deviation of an arm or the like driven by a motor further increases after the arm collides with or comes into contact with an object. By using such control, the robot driving device 1 according to the first embodiment can prevent accidents that damage an object when the arm collides with or comes into contact with the object.

[0040] Embodiment 2 In the second embodiment, a correction processing unit 10a will be described which is another embodiment of the correction processing unit 10 in the first embodiment. In the second embodiment, the components described in the first embodiment will be denoted by the same reference numerals as in the first embodiment, and the description thereof will be omitted.

[0041] Fig. 6 shows a block diagram of a correction processing unit 10a according to the second embodiment. As shown in Fig. 6, the correction processing unit 10a according to the second embodiment is obtained by replacing the correction position calculation unit 31, the correction Kp calculation unit 32, and the filter processing unit 35 with a correction parameter calculation unit 51 and a filter processing unit 55. The correction parameter calculation unit 51 performs the first correction processing performed by the correction position calculation unit 31 and the second correction processing performed by the correction Kp calculation unit 32.

[0042] Here, the correction parameter calculation unit 51 performs a first correction process to calculate a corrected position target value Pos_ref_c[n] based on equations (1) and (2) if the low-frequency side estimated torque τcur_L is equal to or greater than the high-frequency side estimated torque τcur_H. In addition to the first correction process, the correction parameter calculation unit 51 sets a preset initial value (for example, virtual spring constant Kp_ref) to the corrected virtual spring constant Kp_ref_c[n].

[0043] Furthermore, the correction parameter calculation unit 51 performs a second correction process to calculate a corrected virtual spring constant Kp_ref_c[n] based on equations (3) and (5) if the low-frequency side estimated torque τcur_L is smaller than the high-frequency side estimated torque τcur_H. In addition to the second correction process, the correction parameter calculation unit 51 sets a preset initial value (for example, the position target value Pos_ref) to the corrected position target value Pos_ref_c[n].

[0044] The filter processing unit 55 replaces the subtractor 45 with a subtractor 65. In the filter processing unit 55, the subtractor 45 subtracts the high frequency side current position value Pos_cur_H from the corrected previous position target value Pos_ref_c[n-1] to calculate the high frequency side position deviation Pos_σ_H. In the second embodiment, the corrected previous position target value Pos_ref_c[n-1] calculated in the previous cycle is used to calculate the correction value of the virtual spring constant Kp_ref, so that the correction parameter calculation unit 51 can perform the correction process on either the position target value Pos_ref or the virtual spring constant Kp_ref based on the magnitude relationship between the high frequency side estimated torque τcur_H and the low frequency side estimated torque τcur_L.

[0045] Thus, the operation of the correction processing unit 10a according to the second embodiment differs from the operation of the correction processing unit 10a according to the second embodiment in the process flow for calculating the correction value. Therefore, the operation of the correction processing unit 10a according to the second embodiment will be described with reference to the flowchart shown in FIG.

[0046] 7, when the correction processing unit 10a starts the process, first, it acquires the current position value Pos_cur (step S11). Then, the correction processing unit 10a calculates the low-frequency side current position value Pos_cur_L from the current position value Pos_cur in the low-pass filter 41 (step S12). Also, the correction processing unit 10a calculates the high-frequency side current position value Pos_cur_H from the current position value Pos_cur in the hyper-filter 44 (step S13).

[0047] Next, the correction processing unit 10a calculates the low-frequency side estimated torque τcur_L using the corrected previous position target value Pos_ref_c[n-1], the low-frequency side current position value Pos_cur_L, and the corrected previous virtual spring constant value Kp_ref_c[n-1] in the filter processing unit 55 (step S14). The correction processing unit 10a also calculates the high-frequency side position deviation Pos_σ_H using the corrected previous position target value Pos_ref_c[n-1] and the high-frequency side current position value Pos_cur_H in the filter processing unit 55 (step S15). The correction processing unit 10a then calculates the high-frequency side estimated torque τcur_H using the corrected previous virtual spring constant value Kp_ref_c[n-1] and the high-frequency side position deviation Pos_σ_H in the filter processing unit 55 (step S16).

[0048] After that, the correction processing unit 10a compares the low-frequency side estimated torque τcur_L with the high-frequency side estimated torque τcur_H in the correction parameter calculation unit 51 (step S17). If the low-frequency side estimated torque τcur_L is equal to or greater than the high-frequency side estimated torque τcur_H, the correction target position value Pos_ref_c[n] is calculated by a first correction process (step S18). Although not shown, in step S18, the correction processing unit 10a outputs the virtual spring constant Kp_ref as the corrected virtual spring constant Kp_ref_c[n] by a first supplementary correction process. On the other hand, if the low-frequency side estimated torque τcur_L is smaller than the high-frequency side estimated torque τcur_H, the correction target position value Pos_ref_c[n] is calculated by a second correction process (step S19). Although not shown, in step S19, the correction processing unit 10a performs a second supplementary correction process to output the position target value Pos_ref as the corrected position target value Pos_ref_c[n].

[0049] From the above description, the robot driving device including the correction processing unit 10a according to the second embodiment requires less calculations to calculate the corrected position target value Pos_ref_c and the corrected virtual spring constant Kp_ref_c than the correction processing unit 10 according to the first embodiment. As a result, the correction processing unit 10a according to the second embodiment can reduce the calculation load more than the correction processing unit 10 of the first embodiment. Furthermore, since the correction processing unit 10a according to the second embodiment has a smaller calculation load than the correction processing unit 10 of the first embodiment, it is possible to operate at a faster cycle.

[0050] The invention made by the inventor has been specifically described above based on the embodiments, but it goes without saying that the present invention is not limited to the embodiments already described, and various modifications are possible without departing from the gist of the invention. [Explanation of symbols]

[0051] 1 Robot drive unit 10, 10a Correction processing section 11 Control command value generator 12 Motor drive circuit 21, 24, 42, 45, 65 Subtractor 22, 25, 43, 46 multipliers 23 Current speed calculation unit 26, 27 Adder 31 Correction position calculation section 32 Corrected Kp calculation section 33, 34 Delay circuit 34 Delay circuit 35 Filter processing section 41 Low-pass filter 44 Hyperfilter 51 Correction parameter calculation unit 55 Filter processing section Pos_cur Current position value Pos_ref Position target value Pos_ref_c Corrected position target value Kp_ref Virtual spring constant Kp_ref_c Corrected virtual spring constant Vel_ref Speed ​​target value Kd_ref control gain τref Torque target value τcur_L Low frequency side estimated torque τcur_H High frequency side estimated torque

Claims

1. a motor drive circuit that increases or decreases a drive current given to a motor attached to a joint of the robot in accordance with a control command value, and outputs a current position value corresponding to a rotation angle of the motor; a control command value generating unit that generates the control command value using at least the current position value, a position target value of a joint of the robot, a virtual spring constant, a torque target value, and a speed target value; a correction processing unit that corrects the position target value and the virtual spring constant and outputs a corrected position target value and a corrected virtual spring constant to the control command value generation unit in a period synchronized with the periodic processing of the control command value generation unit, The correction processing unit: A robot driving device that applies a position control method that applies a correction to the position target value for low-frequency disturbance components of the disturbance applied to the motor, and applies a virtual spring constant control method that applies a correction to a virtual spring constant for high-frequency disturbance components.

2. The correction processing unit: A reference process for referring to a disturbance indicator target value, which is a preset value and is a target value of a disturbance indicator that varies due to a disturbance applied to the motor; a first correction process for setting a value calculated based on a preset calculation formula to the corrected position target value if the low-frequency disturbance component having a frequency characteristic on the low-frequency side among the components of the disturbance index is equal to or greater than the disturbance index target value, and setting a preset initial value to the corrected position target value if the low-frequency disturbance component is smaller than the disturbance index target value; and a second correction process for setting the corrected virtual spring constant to a value calculated based on a preset calculation formula if the high-frequency disturbance component having a high-frequency characteristic among the components of the disturbance index is equal to or greater than the disturbance index target value, and setting the corrected virtual spring constant to a preset initial value if the low-frequency disturbance component is smaller than the disturbance index target value.

3. The correction processing unit:

3. The robot driving device according to claim 2, wherein the second correction process is performed by referring to the position target value corrected in the first correction process.

4. The correction processing unit: When the low-frequency disturbance component is equal to or greater than the high-frequency disturbance component, a first supplementary correction process is performed in addition to the first correction process, in which a preset initial value is set to the corrected virtual spring constant; The robot driving device according to claim 2, wherein, when the low-frequency disturbance component is smaller than the high-frequency disturbance component, in addition to the second correction process, a second supplementary correction process is performed in which a predetermined initial value is set to the corrected position target value.

5. the disturbance index is a position deviation of the joint or a torque deviation of the motor, 3. The robot driving device according to claim 2, wherein the correction processing section includes a filter processing section that separates the position deviation of the joint or the torque deviation of the motor into high-frequency and low-frequency components.

6. a motor drive circuit that increases or decreases a drive current given to a motor attached to a joint of the robot in accordance with a control command value, and outputs a current position value corresponding to a rotation angle of the motor; a control command value generating unit that generates the control command value using at least the current position value, a position target value of a joint of the robot, a virtual spring constant, a torque target value, and a speed target value; a correction processing unit that corrects the position target value and the virtual spring constant and outputs the corrected position target value and the corrected virtual spring constant to the control command value generation unit in a cycle synchronized with a cycle processing of the control command value generation unit, In the correction processing unit, A robot driving method in which a position control method is applied to add a correction to the position target value for low-frequency disturbance components of the disturbances applied to the motor, and a virtual spring constant control method is applied to add a correction to a virtual spring constant for high-frequency disturbance components.

7. a motor drive circuit that increases or decreases a drive current given to a motor attached to a joint of the robot in accordance with a control command value, and outputs a current position value corresponding to a rotation angle of the motor; a control command value generating unit that generates the control command value using at least the current position value, a position target value of a joint of the robot, a virtual spring constant, a torque target value, and a speed target value; a correction processing unit that corrects the position target value and the virtual spring constant and outputs the corrected position target value and the corrected virtual spring constant to the control command value generation unit in a cycle synchronized with the cycle processing of the control command value generation unit, and a calculation unit that executes calculation processing in the correction processing unit, A robot drive program that applies a position control method that applies a correction to the position target value for low-frequency disturbance components of the disturbances applied to the motor, and applies a virtual spring constant control method that applies a correction to a virtual spring constant for high-frequency disturbance components.

Images