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 drive device, a robot drive method, and a robot drive program, and more particularly, to a robot drive device, a robot drive method, and a robot drive program that control a robot using a virtual spring constant and a position target.

Background Art

[0002] In a robot having an arm that contacts an object, when the arm contacts or collides with the object, the amount of change in the input torque is large and the frequency of the change is high, so there is a problem that the control tends to become unstable. Therefore, a technique for stabilizing such unstable control is disclosed in Patent Document 1.

[0003] The robot described in Patent Document 1 includes a main body part, a plurality of moving mechanism parts each including a moving mechanism that moves on a floor surface, and one or a plurality of joint parts, and a plurality of legs that connect between the main body part and each of the moving mechanism parts, respectively. The joint part includes a drive shaft driven by impedance control.

Prior Art Documents

Patent Documents

[0004]

Patent Document 1

Summary of the Invention

Problems to be Solved by the Invention

[0005] However, in the position control described in Patent Document 1, it is necessary to set the virtual spring constant relatively high in order to guarantee the accuracy of the position control, and if the accuracy of calculating the current position is low, it is difficult to change the target position and there is a problem that it is prone to vibration. In addition, in spring constant control, there is a lower limit to the value that the virtual spring constant can take, which presents a problem that position deviations cannot be tolerated. Thus, in the robot described in Patent Document 1, there is a limitation in the control parameters that can be used to stabilize the position control and torque control, so there is a problem that it is difficult to sufficiently stabilize the position control and torque control.

[0006] This invention was made to solve these problems and aims to stabilize the position control of drive units such as robot arms. [Means for solving the problem]

[0007] One embodiment of the robot drive device according to the present invention includes: a motor drive circuit that increases or decreases the drive current supplied to a motor attached to the joint of a robot according to a control command value and outputs a current position value corresponding to the rotation angle of the motor; a control command value generation unit that generates the control command value using at least the current position value, the target position value of the robot joint, a virtual spring constant, a torque target value, and a speed target value; and a correction processing unit that outputs a corrected target position value and a corrected virtual spring constant to the control command value generation unit at a period synchronized with the periodic processing of the control command value generation unit, wherein the correction processing unit applies a position control method that applies a correction to the target position value for low-frequency disturbance components with low frequencies among the disturbances applied to the motor, and applies a virtual spring constant control method that applies a correction to the virtual spring constant for high-frequency disturbance components with high frequencies.

[0008] One embodiment of the robot driving method according to the present invention is a robot driving method in a robot driving device comprising: a motor driving circuit that increases or decreases the drive current supplied to a motor attached to the joint of a robot according to a control command value and outputs a current position value corresponding to the rotation angle of the motor; a control command value generation unit that generates the control command value using at least the current position value, the target position value of the robot joint, a virtual spring constant, a torque target value, and a speed target value; and a correction processing unit that outputs a corrected target position value and a corrected virtual spring constant to the control command value generation unit at a period synchronized with the periodic processing of the control command value generation unit, wherein the correction processing unit applies a position control method that applies a correction to the target position value for low-frequency disturbance components with low frequencies among the disturbances applied to the motor, and applies a virtual spring constant control method that applies a correction to the virtual spring constant for high-frequency disturbance components with high frequencies.

[0009] One embodiment of the robot drive program according to the present invention includes a motor drive circuit that increases or decreases the drive current supplied to a motor attached to the joint of a robot according to a control command value and outputs a current position value corresponding to the rotation angle of the motor; a control command value generation unit that generates the control command value using at least the current position value, the target position value of the robot joint, a virtual spring constant, a torque target value, and a speed target value; and a correction processing unit that outputs a corrected target position value and a corrected virtual spring constant to the control command value generation unit at a period synchronized with the periodic processing of the control command value generation unit, wherein the robot drive device has a calculation unit that performs calculation processing in the correction processing unit, and applies a position control method that applies a correction to the target position value for low-frequency disturbance components with low frequencies among the disturbances applied to the motor, and applies a virtual spring constant control method that applies a correction to the virtual spring constant for high-frequency disturbance components with high frequencies.

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

[0011] This invention makes it possible to improve the stability of robot position control. [Brief explanation of the drawing]

[0012] [Figure 1] This is a block diagram of a robot drive device according to Embodiment 1. [Figure 2] This diagram illustrates the control logic for changing the target position value. [Figure 3] This diagram illustrates the control logic for changing the virtual spring constant. [Figure 4] This is a block diagram of the correction processing unit according to Embodiment 1. [Figure 5] This is a flowchart illustrating the operation of the correction processing unit according to Embodiment 1. [Figure 6] This is a block diagram of the correction processing unit according to Embodiment 2. [Figure 7] This is a flowchart illustrating the operation of the correction processing unit according to Embodiment 2. [Modes for carrying out the invention]

[0013] For clarity of explanation, the following descriptions and drawings have been omitted and simplified as appropriate. Furthermore, each element shown in the drawings as a functional block performing various processes can be composed of a CPU (Central Processing Unit), memory, and other circuits in hardware terms, and implemented in software terms by programs loaded into memory. Therefore, it will be understood by those skilled in the art that these functional blocks can be implemented in various ways using hardware alone, software alone, or a combination thereof, and are not limited to any one of these. In each drawing, the same elements are denoted by the same reference numeral, and redundant explanations have been omitted where necessary.

[0014] Furthermore, the program described above includes, when loaded into a computer, a set of instructions (or software code) for causing the computer to perform one or more of the functions described in the embodiments. The program may be stored in a non-temporary computer-readable medium or a physical storage medium. Examples, but not limited to, include random-access memory (RAM), read-only memory (ROM), flash memory, solid-state drive (SSD) or other memory technologies, CD-ROM, digital versatile disc (DVD), Blu-ray® disc or other optical disc storage, magnetic cassette, magnetic tape, magnetic disk storage or other magnetic storage devices. The program may be transmitted over a temporary computer-readable medium or a communication medium. Examples, but not limited to, include temporary computer-readable medium or a communication medium that includes electrically, optically, acoustically, or otherwise propagating signals.

[0015] Embodiment 1 FIG. 1 shows a block diagram of a robot drive device 1 according to Embodiment 1. The example shown in FIG. 1 operates a robot by driving a motor provided at a joint of a robot arm or the like. As shown in FIG. 1, the robot drive device 1 according to Embodiment 1 includes a correction processing unit 10, a control command value generation unit 11, and a motor drive circuit 12. In the robot drive 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 given from a higher-level system not shown in the figure. Also, in the robot drive device 1, the control gain Kd_ref may not be given.

[0016] The motor drive circuit 12 increases or decreases the drive current I_od applied to the motor attached to the joint of the robot according to the control command value, and outputs a position current value Pos_cur corresponding to the rotation angle of the motor. The control command value generation unit 11 and the correction processing unit 10 generate a control command value using at least the position current 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 speed 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 cycle synchronized with the cycle processing of the control command value generation unit 11. In Embodiment 1, 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 generation unit 11. Specifically, FIG. 1 shows an example in which the control command value generation unit 11 includes a subtractor 21, a multiplier 22, a current speed calculation 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 and outputs the position deviation. The multiplier 22 multiplies the position deviation output by the subtractor 21 by the corrected virtual spring constant Kp_ref_c and outputs the result. The current speed calculation unit 23 differentiates the current position value Pos_cur and outputs the current speed value. The subtractor 24 subtracts the current speed value from the speed target value Vel_ref and outputs the speed deviation. The multiplier 25 multiplies the speed deviation output by the subtractor 24 by the control gain Kd_ref and outputs the result. The adder 26 adds the torque target value τref to the value output by the multiplier 25 and outputs the result. The adder 27 adds the value output by the multiplier 22 to the value output by the adder 26 and outputs the control command value given to the motor drive circuit 12.

[0019] In the robot drive device 1 according to the first embodiment, in the correction processing unit 10, for the low-frequency side disturbance component with a low frequency among the disturbances applied to the motor, a position control method that corrects the position target value Pos_ref is applied, and for the high-frequency side disturbance component with a high frequency, a correction process is performed using a virtual spring constant control method that corrects the virtual spring constant Kp_ref. Specifically, the correction processing unit 10 performs correction corresponding to the disturbance factors 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 of switching the correction process using the parameter that indicates torque among the parameters that vary due to the disturbance applied to the motor will be described, but any parameter that includes components with different frequencies among the parameters that can capture the changes occurring in the motor due to the disturbance can be used for switching the correction process.

[0020] More specifically, in the robot drive 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 the position target value for the low-frequency side torque fluctuation is performed, and a second correction process using a virtual spring constant control method that corrects the virtual spring constant for the high-frequency side torque fluctuation is performed. Therefore, the control logics of the position control method and the virtual spring constant control method will be described.

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

[0022] Next, Figure 3 shows a diagram illustrating the control logic when the virtual spring constant is changed (virtual spring constant control method). In the example shown in Figure 3, the target position value is assumed to be kept constant. In the example shown in Figure 3, the estimated torque τcur reaches the torque target value τref when the difference between the current position and the target position value Pos_ref reaches the position difference P1. Then, in the virtual spring constant method, if the difference between the current position and the target position value Pos_ref increases further, the virtual spring constant Kp is corrected so that it gradually becomes smaller than the virtual spring constant Kp_ref according to the magnitude of the position difference. If the virtual spring constant control method is not applied, when the position difference between the current position value and the target position 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 closer to the target position value Pos_ref (uncorrected torque in Figure 2). On the other hand, when this virtual spring constant control method is applied, even if the position difference between the current position and the target position Pos_ref increases beyond the position difference P1, the corrected estimated torque is maintained at the torque target value τref.

[0023] As explained in Figures 2 and 3, both the position control method and the virtual spring constant control method can limit the estimated torque to the torque target value τref as the upper limit. However, the position control method is more effective for torque control that fluctuates at low frequencies, while the virtual spring constant control method is more effective for torque control that fluctuates at high frequencies. Therefore, in the robot drive device 1 according to Embodiment 1, the correction processing unit 10 performs a first correction process using the position control method to correct the position target value for torque fluctuations on the low-frequency side, and a second correction process using the virtual spring constant control method to correct the virtual spring constant for torque fluctuations on the high-frequency side. 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, it refers to a pre-set value, which is the target value of the disturbance index that fluctuates due to disturbances applied to the motor. In the following description, the torque value is used as the disturbance index, so the correction processing unit 10 refers to the torque target value τref as the target value of the disturbance index. Furthermore, the correction processing unit 10 uses an estimated value obtained from a calculation using the current position value obtained from the motor as the motor torque. This estimated torque value is referred to as the estimated torque in the following description. In the correction processing unit 10 described below, an example is given in which the estimated torque is calculated using the current position value Pos_cur output by the motor drive circuit 12 based on the rotation angle acquired in motor control, but values ​​obtained from parameters acquired using a torque sensor or current sensor can also be used as the estimated torque described below. Note that since torque sensors have a low response frequency (for example, 1 kHz or less), the torque value obtained from a torque sensor is suitable as a substitute value for the low-frequency side estimated torque τcur_L described below. Furthermore, because current sensors have a high response frequency (for example, 200 kHz or less), the estimated torque calculated from the current value obtained from the current sensor is suitable as a substitute value for the high-frequency side estimated torque τcur_H, which will be explained below.

[0025] In the first correction process, if the low-frequency disturbance component (e.g., low-frequency estimated torque τcur_L) among the components of the disturbance index (estimated torque) that has a low-frequency frequency characteristic is greater than or equal to the disturbance index target value (e.g., torque target value τref), the value calculated based on a pre-set calculation formula is set to the corrected position target value Pos_ref_c. If the low-frequency disturbance component is less than the disturbance index target value, the pre-set 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 the high-frequency side disturbance component (e.g., high-frequency side estimated torque τcur_H) among the components of the disturbance index (estimated torque) that has a frequency characteristic on the high-frequency side is equal to or greater than the disturbance index target value (e.g., torque target value τref), then the value calculated based on a pre-set calculation formula is set to the corrected virtual spring constant Kp_ref_c. high frequency If the lateral disturbance component is smaller than the disturbance index target value, the pre-set initial value is set to the corrected virtual spring constant Kp_ref_c.

[0027] An example configuration of the correction processing unit 10 that realizes the above operation will be described. Hereinafter, Figure 4 shows a block diagram of the correction processing unit 10 according to Embodiment 1. As shown in Figure 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 drive device 1 according to Embodiment 1, since the control command value generation unit 11 generates control command values ​​to be given to the motor drive circuit 12 at a predetermined period, 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 Figure 4 and the following description, n is shown as the current period and n-1 as the value of the previous period.

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

[0029] The corrected Kp calculation unit 32 performs a reference process and a second correction process. Specifically, in the reference process, the corrected Kp calculation unit 32 refers to the given torque target value τref. The corrected Kp calculation unit 32 also compares the torque target value τref with the high-frequency side estimated torque τcur_H calculated by the filter processing unit 35, and if the high-frequency side estimated torque τcur_H is greater than or equal to the torque target value τref, it calculates the corrected virtual spring constant Kp_ref_c, which is the corrected value of the virtual spring constant Kp_ref, based on equation (3). Kp_ref[n]=τref / (Pos_σ_H)···(3) Note that 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 equation (4), Pos_cur_L is the current position value on the high frequency side, Pos_cur_H. On the other hand, if the estimated torque τcur_H on the high frequency side is smaller than the torque target value τref, the corrected virtual spring constant Kp_ref_c, which is a correction value of the virtual spring constant Kp_ref, is calculated by the corrected virtual spring constant Kp_ref_c based on equation (5). Kp_ref_c[n]=Kp_ref···(5) In other words, if the high-frequency estimated torque τcur_H is smaller than the torque target value τref, the corrected virtual spring constant Kp_ref_c is returned to or maintained as the virtual spring constant Kp_ref.

[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 corrected Kp calculation unit 32 until the next processing cycle.

[0031] The filter processing unit 35 separates the joint position deviation or motor torque deviation into high-frequency and low-frequency components. Specifically, the filter processing unit 35 obtains the current position value Pos_cur from the motor, filters the obtained current position value Pos_cur, and calculates the low-frequency current position value Pos_cur_L and the high-frequency current position value Pos_cur_H. Then, the filter processing unit 35 calculates the low-frequency position deviation and the low-frequency estimated torque τcur_L using the low-frequency current position value Pos_cur_L. Furthermore, the filter processing unit 35 calculates the high-frequency position deviation Pos_σ_H and the high-frequency estimated torque τcur_H using the high-frequency current position value Pos_cur_H.

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

[0033] The hyper filter 44 applies a high-pass filter to 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 corrected virtual spring constant previous 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 in which the above processing is performed will be explained in detail. Figure 5 shows a flowchart illustrating the operation of the correction processing unit 10 according to Embodiment 1. The correction processing unit 10 repeats the process shown in Figure 5 at a frequency synchronized with the processing cycle of the control command value generation unit 11.

[0035] As shown in Figure 5, when the correction processing unit 10 starts processing, it first obtains the current position value Pos_cur (step S1). Next, 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 estimated torque τcur_L in the filter processing unit 35 using the corrected target position previous value Pos_ref_c[n-1], the low-frequency current position value Pos_cur_L, and the corrected virtual spring constant previous value Kp_ref_c[n-1] (step S4). After that, the correction processing unit 10 calculates the corrected target position value Pos_ref_c[n] in the corrected position calculation unit 31 (step S5). In step S5, the corrected position calculation unit 31 selects one of the calculation formulas (1) or (2) based on the relationship between the low-frequency estimated torque τcur_L and the torque target value τref to calculate the corrected target position 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 current high-frequency side position value Pos_cur_H (step S6). Also, the correction processing unit 10 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 corrected Kp calculation unit 32 (step S8). In this step S8, the corrected Kp calculation unit 32 selects one of the calculation formulas (3) or (5) based on the 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 drive device 1 according to Embodiment 1, the correction processing unit 10 separates the torque fluctuation caused by disturbances into low-frequency and high-frequency components, applies a position control method to the low-frequency component, and applies a virtual spring constant control method to the high-frequency component. As a result, the robot drive device 1 according to Embodiment 1 eliminates the limitations of control methods that apply only one of the position control method or the virtual spring constant control method. Furthermore, the robot drive device 1 according to Embodiment 1 can achieve higher stability compared to control methods that apply only one of the position control method or the virtual spring constant control method.

[0039] Specifically, the robot drive device 1 according to Embodiment 1 can stably control the position and torque when the motor-driven arm or the like collides with or contacts an object. Furthermore, the robot drive device 1 according to Embodiment 1 can keep the torque constant even when the position deviation increases further after the motor-driven arm or the like collides with or contacts an object. Through such control, the robot drive device 1 according to Embodiment 1 can prevent accidents that would damage an object when the arm collides with or contacts it.

[0040] Embodiment 2 Embodiment 2 describes a correction processing unit 10a, which is another form of the correction processing unit 10 of Embodiment 1. In the description of Embodiment 2, the same reference numerals as in Embodiment 1 are used for the components described in the description of Embodiment 1, and their descriptions are omitted.

[0041] Figure 6 shows a block diagram of the correction processing unit 10a according to Embodiment 2. As shown in Figure 6, the correction processing unit 10a according to Embodiment 2 replaces 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 that was performed by the correction position calculation unit 31 and the second correction processing that was performed by the correction Kp calculation unit 32.

[0042] Here, the correction parameter calculation unit 51 performs a first correction process to calculate the corrected position target value Pos_ref_c[n] based on equations (1) and (2) if the low-frequency estimated torque τcur_L is equal to or greater than the high-frequency estimated torque τcur_H. In addition, along with 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 the corrected rear virtual spring constant Kp_ref_c[n] based on equations (3) and (5) if the estimated torque τcur_L on the low frequency side is smaller than the estimated torque τcur_H on the high frequency side. In addition, along with 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 high-frequency side position deviation Pos_σ_H is calculated by subtracting the current high-frequency side position value Pos_cur_H from the previous corrected position target value Pos_ref_c[n-1] in the subtractor 45. In this embodiment 2, by using the previous corrected position target value Pos_ref_c[n-1] calculated in the previous cycle to calculate the correction value of the virtual spring constant Kp_ref, the correction parameter calculation unit 51 can perform correction processing on either the position target value Pos_ref or the virtual spring constant Kp_ref based on the relationship between the estimated high-frequency side torque τcur_H and the estimated low-frequency side torque τcur_L.

[0045] As described above, the operation of the correction processing unit 10a in Embodiment 2 differs from the processing flow for calculating the correction value. Therefore, Figure 7 shows a flowchart illustrating the operation of the correction processing unit 10a in Embodiment 2, and the operation of the correction processing unit 10a will be explained.

[0046] As shown in Figure 7, when the correction processing unit 10a starts processing, it first obtains the current position value Pos_cur (step S11). Next, 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). Furthermore, 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 estimated low-frequency torque τcur_L in the filter processing unit 55 using the corrected previous target position value Pos_ref_c[n-1], the current low-frequency position value Pos_cur_L, and the corrected previous virtual spring constant value Kp_ref_c[n-1] (step S14). Also, the correction processing unit 10a calculates the high-frequency position deviation Pos_σ_H in the filter processing unit 55 using the corrected previous target position value Pos_ref_c[n-1] and the current high-frequency position value Pos_cur_H (step S15). Then, the correction processing unit 10a calculates the estimated high-frequency torque τcur_H in the filter processing unit 55 using the corrected previous virtual spring constant value Kp_ref_c[n-1] and the high-frequency position deviation Pos_σ_H (step S16).

[0048] Subsequently, the correction processing unit 10a compares the magnitudes of the low-frequency estimated torque τcur_L and the high-frequency estimated torque τcur_H in the correction parameter calculation unit 51 (step S17). If the low-frequency estimated torque τcur_L is greater than or equal to the high-frequency estimated torque τcur_H, the first correction process calculates the corrected position target value Pos_ref_c[n] (step S18). Although not shown in the figures, 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] through the first supplementary correction process. On the other hand, if the low-frequency estimated torque τcur_L is smaller than the high-frequency estimated torque τcur_H, the second correction process calculates the corrected virtual spring constant Kp_ref_c[n] (step S19). Although not shown in the diagram, in step S19, the correction processing unit 10a outputs the position target value Pos_ref as the corrected position target value Pos_ref_c[n] through a second supplementary correction process.

[0049] As described above, the robot drive device including the correction processing unit 10a according to Embodiment 2 requires less computation 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 Embodiment 1. As a result, the correction processing unit 10a according to Embodiment 2 can reduce the computational load compared to the correction processing unit 10 of Embodiment 1. Furthermore, because the correction processing unit 10a according to Embodiment 2 has a lower computational load than the correction processing unit 10 of Embodiment 1, it can operate at a faster cycle.

[0050] The present invention has been described in detail above based on 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 spirit of the invention. [Explanation of symbols]

[0051] 1. Robot drive system 10, 10a Correction processing unit 11 Control command value generation unit 12 Motor drive circuit 21, 24, 42, 45, 65 Subtractors 22, 25, 43, 46 multipliers 23 Current Speed ​​Calculation Unit 26, 27 Adder 31 Correction position calculation section 32 Correction Kp Calculation Unit 33, 34 Delay Circuit 34 Delay Circuit 35 Filtering section 41 Low-pass filter 44 Hyper Filters 51 Correction parameter calculation unit 55 Filter Processing Unit Pos_cur current position 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 estimated torque τcur_H Estimated torque on the high-frequency side

Claims

1. A motor drive circuit that increases or decreases the drive current supplied to a motor attached to a robot joint according to a control command value, and outputs a current position value corresponding to the rotation angle of the motor, At a minimum, a control command value generation unit generates the control command value using the current position value, the target position value of the robot's joints, a virtual spring constant, a torque target value, and a velocity target value, The system includes a correction processing unit that, in a periodic manner synchronized with the periodic processing of the control command value generation unit, outputs a corrected position target value and a corrected virtual spring constant to the control command value generation unit, obtained by correcting the position target value and the virtual spring constant. The correction processing unit, A robot drive device that applies a position control method to correct the position target value for low-frequency disturbance components among the disturbances applied to the motor, and applies a virtual spring constant control method to correct the virtual spring constant for high-frequency disturbance components.

2. The correction processing unit, A reference process that refers to a pre-set value, which is a target value of the disturbance index that fluctuates due to disturbances applied to the motor, and A first correction process is performed in which, if the low-frequency disturbance component having low-frequency characteristics among the components of the disturbance index is equal to or greater than the disturbance index target value, a value calculated based on a predetermined calculation formula is set as the corrected position target value, and if the low-frequency disturbance component is smaller than the disturbance index target value, an initial value predetermined is set for the corrected position target value. The robot drive device according to claim 1, which performs a second correction process in which, if the high-frequency side disturbance component having high-frequency characteristics among the components of the disturbance index is equal to or greater than the target value of the disturbance index, a value calculated based on a preset calculation formula is set as the corrected virtual spring constant, and if the high-frequency side disturbance component is smaller than the target value of the disturbance index, a preset initial value is set as the corrected virtual spring constant.

3. The correction processing unit, The robot drive device according to claim 2, which performs the second correction process by referring to the position target value corrected in the first correction process.

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

5. The disturbance index is the position deviation of the joint or the torque deviation of the motor. The robot drive device according to claim 2, wherein the correction processing unit has a filter processing unit 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 the drive current supplied to a motor attached to a robot joint according to a control command value, and outputs a current position value corresponding to the rotation angle of the motor, At a minimum, a control command value generation unit generates the control command value using the current position value, the target position value of the robot's joints, a virtual spring constant, a torque target value, and a velocity target value, A robot drive method in a robot drive device, comprising: a correction processing unit that outputs a corrected position target value and a corrected virtual spring constant to the control command value generation unit at a periodic interval synchronized with the periodic processing of the control command value generation unit, wherein the position target value and the virtual spring constant have been corrected, and the corrected virtual spring constant is output to the control command value generation unit; In the correction processing unit, A robot drive method that applies a position control method to correct the position target value for low-frequency disturbance components of the disturbances applied to the motor, and applies a virtual spring constant control method to correct the virtual spring constant for high-frequency disturbance components.

7. A motor drive circuit that increases or decreases the drive current supplied to a motor attached to a robot joint according to a control command value, and outputs a current position value corresponding to the rotation angle of the motor, At a minimum, a control command value generation unit generates the control command value using the current position value, the target position value of the robot's joints, a virtual spring constant, a torque target value, and a velocity target value, A robot drive device having 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 at a period synchronized with the periodic processing of the control command value generation unit, and a calculation unit that performs calculation processing in the correction processing unit, wherein the robot drive program executed by the calculation unit is: A robot drive program that applies a position control method to correct the position target value for low-frequency disturbance components among the disturbances applied to the motor, and applies a virtual spring constant control method to correct the virtual spring constant for high-frequency disturbance components.

Images