Robot control method

Abstract

A robot control method includes a collision detection method for detecting an external force by subtracting a kinetic torque and a friction torque calculated value obtained by an inverse kinetic calculation of the robot from a torque output from a motor to a decelerator in a robot driven by the motor via the decelerator. When the detected value of the external force is higher than a predetermined threshold value, it is determined that the robot has collided. In the robot control method, when it is determined that there is no collision, a friction estimated filter value obtained by smoothing a friction torque estimated value used for setting a friction torque calculation value is updated on the basis of a friction estimated filter value prior to a predetermined time. The friction estimation filter value before and after the update is stored as a friction estimation stored value. When it is determined that there is no collision, the friction estimation filter value is returned to the friction estimation saved value before update.

Description

Technical Field

[0001] The present disclosure relates to a robot control method.

[0002] Patent document 1 discloses a collision detection and stop control method. When a connecting rod of a robot composed of multiple joints, a tool or workpiece held at the front end of the connecting rod collides with surrounding obstacles, the method detects the collision and immediately stops the robot in order to avoid deformation of the connecting rod or damage to the movable part of the robot.

[0003] Prior art literature

[0004] Patent Literature

[0005] Patent Document 1: Japanese Patent No. 4756618 Summary of the Invention

[0006] The present disclosure is proposed in view of the above-mentioned existing situation, and its purpose is to improve the sensitivity of collision detection of a robot.

[0007] The present disclosure provides a robot control method having a collision detection method. In a robot driven by a motor via a reducer, the collision detection method subtracts the dynamic torque and the friction torque calculation value obtained by inverse dynamics calculation of the robot from the torque output by the motor to the reducer, thereby detecting an external force. When the detected value of the external force is higher than a predetermined given threshold, it is determined that the robot has collided. When it is determined that no collision has occurred, the friction estimation filter value obtained by smoothing the friction torque estimation value used in setting the friction torque calculation value is updated based on the friction estimation filter value before a given time, and the friction estimation filter values ​​before and after the update are saved as friction estimation saved values.

[0008] In addition, these general or specific aspects may be implemented by a system, apparatus, method, integrated circuit, computer program, or recording medium, or by any combination of systems, apparatus, method, integrated circuit, computer program, and recording medium.

[0009] According to the present disclosure, the sensitivity of collision detection of a robot can be improved. BRIEF DESCRIPTION OF THE DRAWINGS

[0010] Figure 1 This is a control block diagram of the existing dynamic calculation method.

[0011] Figure 2 This is a diagram showing the approximate error of the friction torque calculation value.

[0012] Figure 3 This is a block diagram of the control device according to this embodiment.

[0013] Figure 4 This is a flowchart of the friction torque calculation process according to the present embodiment.

[0014] Figure 5 This is a diagram showing a case where the collision detection threshold is smaller than the friction estimation threshold.

[0015] Figure 6 This is a diagram showing a case where the collision detection threshold is greater than the friction estimation threshold.

[0016] Figure 7 This is a control block diagram of the dynamics calculation method according to this embodiment. DETAILED DESCRIPTION

[0017] (Process of Realizing This Embodiment)

[0018] In recent years, robots have been required to achieve higher-precision collision detection to improve safety during collisions and prevent damage caused by breakage. However, using high-precision collision sensors increases costs and places a load on the robot due to the weight of the sensors, which reduces the accuracy of robot control and reliability. For these reasons, there is a demand for high-precision collision detection without sensors.

[0019] As a sensorless collision detection method, there is a dynamic calculation method: the collision force is calculated by subtracting the robot's dynamic torque from the reducer output torque. The reducer output torque is obtained by subtracting the torque lost due to inertia and friction of the motor and reducer from the torque generated by the motor's driving current. The robot's dynamic torque is calculated based on the robot's inverse dynamics calculation.

[0020] To increase the sensitivity of robot collision detection and further reduce damage during collisions, it is necessary to improve the accuracy of calculating the torque related to collisions (hereinafter referred to as "collision torque"). When calculating the collision torque, the torque caused by friction (hereinafter referred to as "friction torque") is calculated.

[0021] Patent Document 1 calculates friction torque by summing the product of the kinematic viscosity coefficient and the motor speed, and the product of the Coulomb friction coefficient and the sign function. In other words, the friction torque is calculated using a linear approximation of the motor speed. Thus, the friction torque values ​​used in collision torque calculations are often calculated using a linear approximation. However, in practice, not only is the approximation error large, but it also increases due to temperature fluctuations and individual differences between robots, leading to a problem in which the accuracy of collision torque calculation deteriorates.

[0022] Hereinafter, the embodiments of the robot control method specifically disclosed in the present disclosure will be described in detail with appropriate reference to the accompanying drawings. However, unnecessary detailed descriptions may be omitted. For example, detailed descriptions of known matters and repeated descriptions of substantially the same structures may be omitted. This is to avoid the following description from becoming unnecessarily lengthy and to facilitate understanding by those skilled in the art. In addition, the drawings and the following description are provided to enable those skilled in the art to fully understand the present disclosure and are not intended to limit the subject matter described in the claims.

[0023] First, refer to Figure 1 The existing dynamic calculation method is described below. Figure 1 This is the control block diagram of the existing dynamic operation method. Figure 1 In the dynamic calculation method shown, the calculation process is executed by a processor (not shown) built into the device that controls the motor.

[0024] In the position control block 6, the speed command is calculated based on the position command. First, the difference between the position command θcom and the position feedback θm of the motor is calculated. The position command θcom is the value obtained by integrating the speed component dθcom of the position command. 1 / s represents the integral element. The position feedback θm is the actual rotation position of the motor detected by the motor rotation angle detector 99 such as an encoder mounted on the motor. Here, the subscript m represents the axis of the robot driven by the motor. For example, in the case where the motor drives a robot with 6 axes, m becomes an integer from 1 to 6 corresponding to each axis. The speed command ωcom is calculated by multiplying the difference between the position command θcom and the position feedback θm of the motor by the position proportional gain KPP.

[0025] The speed control block 10 calculates the motor current based on the speed command. First, the motor angular velocity ωm, the speed feedback value obtained by differentiating the position feedback θm, is calculated. s represents the differential element. The motor current Im is calculated by taking the sum of the difference between the speed command ωcom and the motor angular velocity ωm, multiplied by the speed proportional gain KP, and the difference between the speed command ωcom and the motor angular velocity ωm, multiplied by the speed integral gain KI / s.

[0026] The torque generated in the motor (hereinafter referred to as "motor generated torque") is calculated in the torque control block 18. First, the motor current Im is multiplied by the motor torque constant Kt to calculate the motor generated torque τm.

[0027] [Mathematical formula 1]

[0028] τm=Kt×Im (1)

[0029] Formula (1) represents the torque generated by the motor as viewed from the drive side of the motor. On the other hand, the torque generated by the motor as viewed from the load side of the motor is represented by Formula (2).

[0030] [Mathematical formula 2]

[0031] τm=τdyn+τdis+Jm×αm+D×ωm+τμ (2)

[0032] Here, Kt is the motor torque constant, Im is the motor current, αm is the motor angular acceleration, ωm is the motor angular velocity, Jm is the motor inertia, D is the viscous friction coefficient, τμ is the kinetic friction torque, τdyn is the dynamic torque, and τdis is the collision torque. The dynamic torque τdyn is the sum of the gravitational torque, inertial force, centrifugal force, and Coriolis force.

[0033] In addition, the dynamic friction torque τμ can be calculated by equation (3).

[0034] [Mathematical formula 3]

[0035] τμ=Kμ×sgn(3)

[0036] Here, Kμ represents the magnitude of dynamic friction. sgn is 1 when ωm>0, 0 when ωm=0, and -1 when ωm<0. The sign of ωm is determined by the direction determination 21.

[0037] The collision torque τdis can be obtained by transforming the equations (1) and (2) into the equation (4).

[0038] [Formula 4]

[0039] τdis=Kt×Im-(Jm×αm+D×ωm+τμ+τdyn) (4)

[0040] The collision torque estimation block 30 represents equation (4) as a block diagram. The dynamic torque calculation block 26 performs inverse dynamics calculations on position feedback θm, motor angular acceleration αm, motor angular velocity ωm, and position feedback θm from other axes to calculate the dynamic torque τdyn. The collision torque τdis calculated by the collision torque estimation block 30 according to equation (4) is output to the collision determination block 31.

[0041] A collision is detected using a predetermined collision detection threshold value in the collision determination block 31. When the absolute value of the collision torque τdis is greater than the collision detection threshold value, it is determined that a collision has occurred.

[0042] Here, D×ωm representing viscous friction and τμ representing dynamic friction in equation (4) are collectively referred to as the friction torque calculated value τμcal.

[0043] [Formula 5]

[0044] τμcal=D×ωm+τμ (5)

[0045] Formula (4) becomes Formula (6) according to Formula (5).

[0046] [Formula 6]

[0047] τdis=Kt×Im-(Jm×αm+τμcal+τdyn) (6)

[0048] Thus, the calculation accuracy of the collision torque τdis is affected by the accuracy of the friction torque calculation value τμcal and the dynamic torque τdyn. As shown in equation (5), the friction torque calculation value τμcal is calculated using a linear approximation of the motor angular velocity ωm.

[0049] Next, refer to Figure 2 The approximate error of the friction torque calculation value will be described. Figure 2 This is a diagram showing the approximate error of the friction torque calculation value.

[0050] Figure 2 In the graph shown, the horizontal axis represents the motor angular velocity ω [%], and the vertical axis represents the magnitude of the friction torque.

[0051] Graph W1 is a first-order approximation of the measured values ​​of graph W2. As shown in equation (5), graph W1 has an intercept of τμ and a slope of D when the motor angular velocity ω is positive, and an intercept of -τμ and a slope of D when the motor angular velocity ω is negative.

[0052] Graph W2 shows the values ​​measured using reciprocating constant speed motion. Here, as a method for calculating the friction torque values ​​shown in Graph W2, a method for measuring friction torque by attaching a load to the robot is described. The gravitational torque due to the load attached to the robot is expressed as Equation (7).

[0053] [Formula 7]

[0054] τgL[θr]={Kt×Im[θr]-D×ωm-Kμ×sgn}×Rg-τgA[θr] (7)

[0055] Here, θr is the robot joint angle, Rg is the reduction ratio of the speed reducer, and τgA[θr] is the gravitational torque due to the arm of the robot body. If the current when the robot is operating in the negative direction with sgn = -1 and at an angular velocity of -ωm0 is Im[θr]m, and the motor current when the robot is operating in the positive direction with sgn = 1 and at an angular velocity of ωm0 is Im[θr]p, then Equation (7) becomes Equation (8) and Equation (9), respectively.

[0056] [Formula 8]

[0057] τgL[θr]={Kt×Im[θr]m+D×ωm0-Kμ}×Rg-τgA[θr] (8)

[0058] [Formula 9]

[0059] τgL[θr]={Kt×Im[θr]p+D×ωm0-Kμ}×Rg-τgA[θr] (9)

[0060] Subtracting equation (9) from equation (8) yields the following equation (10).

[0061] [Formula 10]

[0062] 0=Kt×(Im[θr]m-Im[θr]p)+2×(D×ωm0+Kμ) (10)

[0063] Here, when viscous friction and dynamic friction are collectively defined as a friction torque measurement value τμmes, it is expressed by the following equation (11).

[0064] [Mathematical formula 11]

[0065] τμmes[ωm0]=D×ωm0+Kμ (11)

[0066] According to the equations (10) and (11), the friction torque measurement value τμmes is expressed by the following equation (12).

[0067] [Mathematical formula 12]

[0068]

[0069] As described above, the friction torque measured values ​​for the motor angular velocity ωm0 shown in graph W2 are calculated by subtracting the positive and negative motor currents at the same angle θr, multiplying by the motor torque constant Kt, and dividing by 2. The friction torque measured values ​​shown in graph W2, compared to graph W1, which shows the friction torque values ​​after the primary approximation, have an error due to speed.

[0070] In addition, the friction torque measurement value shown in Figure W2 will also change depending on the temperature. For example, when the temperature rises, the friction torque decreases. In addition, when the temperature drops, the friction torque increases. This is because, for example, the hardness of the grease used in the motor changes with temperature, thereby changing the friction torque. In addition, the error in the friction torque measurement value will also increase depending on the individual differences of the motor. Thus, as shown in formula (5), if the friction torque is calculated using the first-order approximation of the motor angular velocity, the error in the calculated friction torque value will increase compared to the actual friction torque value. When the error in the friction torque increases, the error in the collision torque τdis calculated using the friction torque increases, and there is a problem that the sensitivity of the collision detection is reduced. Below, a robot control method proposed in view of such a situation is described, which estimates the friction torque in real time during the movement of the motor to improve the estimation accuracy of the friction torque.

[0071] Next, refer to Figure 3 A block diagram of the control device 100 according to this embodiment will be described. Figure 3 This is a block diagram of the control device 100 according to this embodiment.

[0072] The control device 100 is a device that controls the driving of a motor and includes a communication I / F 101 , a processor 102 , and a memory 103 .

[0073] The communication I / F 101 is an interface circuit for wireless or wired communication between the control device 100 and an external device (not shown). The so-called external device is, for example, a teaching pendant, a robot driven by a motor, an amplifier that supplies current to the motor, or an encoder installed on the motor. Here, I / F represents an interface. The communication method based on the communication I / F 101 is, for example, a wide area network (Wide Area Network), a local area network (Local Area Network), long term evolution (Long Term Evolution), 4G, 5G and other mobile communications, power line communication, short-range wireless communication (such as Bluetooth (registered trademark) communication), or communication for mobile phones.

[0074] The processor 102 is, for example, a central processing unit (CPU), a digital signal processor (DSP), a graphics processing unit (GPU), or a field programmable gate array (FPGA). The processor 102 functions as a controller that manages the overall operation of the control device 100. The processor 102 performs control processing for unifying the operations of the various components of the control device 100, data input and output processing between the various components of the control device 100, data calculation processing, and data storage processing. The processor 102 uses the memory 103 during operation and temporarily stores data generated or acquired by the processor 102 in the memory 103. The processor 102 performs calculations to generate torque for the motor.

[0075] The memory 103 is configured using, for example, a random access memory (RAM) and a read-only memory (ROM), and temporarily stores programs required for the operation of the control device 100 and data generated during the operation. The RAM is, for example, a working memory used in the operation of the control device 100. The ROM stores and stores, for example, programs for controlling the control device 100.

[0076] Next, refer to Figure 4 The calculation process of the friction torque according to this embodiment will be described. Figure 4 This is a flowchart of the friction torque calculation process according to the present embodiment.

[0077] The processor 102 determines whether a collision can be detected with high sensitivity by determining whether the collision detection threshold is smaller than the friction estimation threshold (step St100). The collision detection threshold and the friction estimation threshold are described below.

[0078] The calculated value of the collision torque τdis contains model and parameter errors, and therefore will not be "0" even when no collision occurs. Model error refers to the error that occurs when linearly approximating the characteristics of friction, speed reducers, and other factors. Parameter error refers to errors caused by temperature fluctuations in friction, individual differences, and user settings of the load attached to the robot. The term "user" refers to, for example, the person performing the collision determination.

[0079] Regarding the collision detection threshold, during actual operation in the user's environment, the calculated value of the collision torque τdis is displayed on a teaching pendant, etc. This collision detection threshold is set to a value sufficiently higher than the maximum calculated value of the collision torque τdis when no collision occurs. By allowing the user to set the collision detection threshold in this way, collision sensitivity can be appropriately adjusted without false collision detection.

[0080] By deriving the estimated friction torque value τμest in real time (described later), the error between the actual friction torque and the estimated value is reduced, and the calculated value of the collision torque τdis when no collision occurs approaches "0 (zero)." Consequently, the collision detection threshold (i.e., collision detection sensitivity) setting method described above can be used to lower the collision detection threshold (i.e., increase the collision sensitivity).

[0081] The collision detection threshold (that is, the collision detection sensitivity) is a value set by the user and is therefore not limited to being smaller. For example, when the parameters of the load assembled in the robot (mass, center of gravity position, etc.) are incorrect, errors are generated in the dynamic calculation, and sometimes the collision detection sensitivity cannot be improved. When the collision detection threshold is high, even if a collision occurs and an external force is applied, it is sometimes not determined to be a collision. The friction estimation threshold is a threshold for judging the situation where the collision detection threshold set by the user is smaller (that is, the collision detection sensitivity is higher). For example, when determining whether the collision detection threshold is set to be less than 20% of the maximum torque ratio of the motor (that is, the collision detection sensitivity is more than 80%), the friction estimation threshold is 20%. By comparing the friction estimation threshold with the collision detection threshold, the processor 102 determines whether the collision can be judged with high sensitivity.

[0082] If the processor 102 determines that the collision detection threshold is greater than the friction estimation threshold and that a collision cannot be detected with high sensitivity (step St100, No), the processing ends. If the processor 102 determines that a collision cannot be detected with high sensitivity, it may output a signal to a display device (not shown) or the like urging the user to change the setting of the collision detection threshold.

[0083] If the processor 102 determines that the collision detection threshold is less than the friction estimation threshold and that a collision can be detected with high sensitivity (step St100, yes), it then determines whether a collision has occurred (step St101). If the collision torque τdis is greater than the collision detection threshold, the processor 102 determines whether a collision has occurred.

[0084] If the processor 102 determines that no collision has occurred (step St101, No), it calculates an estimated friction torque value τμest (step St102). Since no collision has occurred, the collision torque τdis is close to zero. The estimated friction torque value τμest is the friction torque calculated value τμcal when the collision torque τdis in equation (6) is considered zero. The estimated friction torque value τμest is derived from equation (13).

[0085] [Mathematical formula 13]

[0086] τμest=Kt×Im-(Jm×αm+τdyn) (13)

[0087] By setting the collision detection threshold lower than the friction estimation threshold, collision detection sensitivity is increased. During periods when no collision is detected, the friction torque estimate τμest can be derived in real time using Equation (13). In Equation (13), the motor current Im is a value that varies with temperature changes and individual motor variations. By deriving the friction torque estimate τμest in real time, processor 102 can prevent errors in the calculated friction torque due to temperature changes and individual motor variations.

[0088] Processor 102 applies filtering (smoothing) to the friction torque estimate for each motor speed to calculate a friction estimate filter value. The filtering process is described in step St104. First, processor 102 calculates a speed array element to calculate the friction estimate filter value (step St103). The speed array element is represented by the following equation (14).

[0089] [Mathematical formula 14]

[0090]

[0091] Here, N is an integer greater than or equal to 1, and j is an integer from 1 to N. The processor 102 calculates a friction estimation filter value using equation (15) for each speed array element obtained in step St103 (step St104 ).

[0092] [Mathematical formula 15]

[0093] τμf[ωj][i]=A×τμf[ωj][j-1]+(1-A)×τμest(15)

[0094] Here, τμf[ωj][i] is the friction estimation filtered value for each speed, A is a filter constant with a value of 0 < A < 1, and i is the sampling period. The sampling period is, for example, about 1 [msec] and is a value determined in advance by the user. In addition, the sampling period is not limited to 1 [msec]. The filter constant is a value that can be set by the user to a value between 0 and 1. The first term on the right side of Equation (15) is a value containing information on the past friction estimation filtered value. The second term on the right side of Equation (15) is the current friction estimation filtered value. That is, the friction estimation filtered value is a value obtained by weighting and adding the past value and the current value using the value of the filter constant. For example, when A = 0.5, the friction estimation filtered value is a value obtained by adding the past value and the current value in the same proportion.

[0095] The processor 102 updates the friction estimation saved value τμreg[ωj][k] stored in the memory 103 at a given time interval (step St105). For example, the processor 102 updates the friction estimation saved value every few minutes. The processor 102 stores the friction estimation filtered value updated in the process of step St105 and the friction estimation filtered value before the update in the memory 103 as the friction estimation saved value. In addition, the time interval for the update is not limited to a few minutes. The friction estimation saved value is represented by Equation (16) and Equation (17).

[0096] [Mathematical formula 16]

[0097] τμreg[ωj][k - 1] = τμreg[ωj][k] (16)

[0098] [Mathematical formula 17]

[0099] τμreg[ωj][k] = τμf[ωj][i] (17)

[0100] Equation (16) is the value of the friction compensation saved value updated last time.

[0101] The processor 102 determines whether the motor angular velocity ωm is 0 or more (that is, the positive or negative of the motor angular velocity ωm) (step St106). The positive or negative of the motor angular velocity ωm is determined according to the rotation direction of the motor.

[0102] When the processor 102 determines that the motor angular velocity ωm is 0 or more (step St106, yes), it sets the friction torque calculation value τμcal to Equation (18) (step St107).

[0103] [Mathematical formula 18]

[0104] τμcal = τμreg[ωj][k - 1] (18)

[0105] When the processor 102 determines that the motor angular velocity ωm is less than 0 (step St106 , No), it sets the friction torque calculation value τμcal to equation (19) (step St108 ).

[0106] [Mathematical formula 19]

[0107] τμcal=-τμreg[ωj][k-1] (19) The processor 102 uses the friction torque calculation value τμcal in equation (18) or equation (19) to calculate the collision torque τdis according to equation (6).

[0108] When the processor 102 determines that a collision has occurred (step St101 , Yes), the processor 102 returns the friction estimation filter value to the value before a predetermined time as shown in equation (20) (step St109 ).

[0109] [Mathematical formula 20]

[0110] τμcal=-τμreg[ωj][k-1](20)

[0111] If a collision occurs, the collision torque τdis is already non-zero before the collision is detected (that is, before the collision torque τdis becomes equal to or greater than the collision detection threshold). Therefore, the friction torque estimate τμest calculated using equation (13) becomes inaccurate, resulting in a large error. However, by returning the friction estimate filter value to the value before a predetermined time based on equation (20) in step St109, the error in the friction estimate filter value τμf[ωj][i] caused by the collision can be eliminated.

[0112] As described above, the robot control method according to this embodiment estimates the friction torque value in real time during motor operation, preventing errors in the estimated friction torque due to temperature changes and individual differences between motors, and thus calculating the collision torque with high accuracy. This method can thus suppress errors in collision torque estimation and improve collision detection sensitivity.

[0113] Next, refer to Figure 5 and Figure 6 The relationship between the collision detection threshold and the friction estimation threshold will be described.

[0114] first, Figure 5 1 is a diagram showing a case where the collision detection threshold value τdisth is smaller than the friction estimation threshold value τμth.

[0115] Graph W3 shows the temporal change of the collision torque τdis. The horizontal axis of graph W3 represents time [seconds], and the vertical axis represents the value of the collision torque τdis.

[0116] exist Figure 5 In the example shown, a collision occurs around 0.4 seconds, and the collision torque τdis begins to increase. A collision is detected around 0.5 seconds, when the collision torque τdis exceeds the collision detection threshold τdisth. Because the collision detection threshold τdisth is lower than the friction estimation threshold τμth, the collision detection sensitivity is high, allowing the control device 100 to detect even weak collisions.

[0117] In addition, region R1 is a region where a collision actually occurred but it is considered that no collision occurred because the collision torque τdis is less than the collision detection threshold τdisth. In region R1, the collision torque τdis is set to zero and the friction torque estimation value is calculated by formula (13). However, in reality, the collision torque τdis is not zero, so the error becomes large. The control device 100 returns the friction estimation filter value to the value before a given time (refer to Figure 4 Step St109) can eliminate the error caused by the collision.

[0118] on the other hand, Figure 6 This is a diagram showing a case where the collision detection threshold τdisth is larger than the friction estimation threshold τμth.

[0119] Graph W4 shows the temporal change of the collision torque τdis. The horizontal axis of graph W4 represents time [seconds], and the vertical axis represents the value of the collision torque τdis.

[0120] exist Figure 6 In the example shown, in region R3, a collision occurs around 0.4 seconds, and the collision torque τdis begins to increase, reaches a maximum around 0.5 seconds, and then decreases. A collision occurs in region R3, but because the collision torque τdis is less than the collision detection threshold τdisth, no collision is detected.

[0121] In region R4, a collision occurs around 0.7 seconds, the collision torque τdis begins to increase, reaches a maximum around 0.8 seconds, and then decreases. Around 0.8 seconds, the collision torque τdis exceeds the collision detection threshold τdisth, and thus a collision is detected.

[0122] Thus, until a strong collision exceeding the collision detection threshold τdisth occurs, no collision is detected. Consequently, weaker collisions, such as those in region R3, remain undetected. In other words, in region R3, even though a collision occurs and the actual collision torque τdis is not zero, the friction torque calculated value is calculated using the collision torque τdis as zero. This increases the error in the friction torque calculated value, degrading collision detection sensitivity.

[0123] For this reason, when the collision detection threshold τdisth is equal to or greater than the friction estimation threshold τμth, it is considered that a collision cannot be determined with high sensitivity, and the collision detection threshold τdisth is set to a value smaller than the friction estimation threshold τμtth.

[0124] Next, refer to Figure 7 A control block diagram of the dynamics calculation method according to this embodiment will be described. Figure 7 This is a control block diagram of the dynamic calculation method involved in this embodiment. The internal structure of the collision torque estimation block 30 is the same as Figure 1 The control block diagrams are different, so the description of the same parts is omitted and only the different parts are described.

[0125] The collision torque estimation block 30 estimates the friction torque estimate τμest. As shown in equation (13), the friction torque estimate τμest is obtained by multiplying the motor current Im by the motor torque constant Kt, subtracting the value obtained by multiplying the motor inertia Jm by the motor angular acceleration am, and the value of the dynamic torque τdyn calculated by the dynamic torque calculation block 26.

[0126] The friction torque estimate τμest is filtered by the friction estimation block 27 to calculate the friction torque calculated value τμcal. As shown in equation (6), the collision torque τdis is calculated by subtracting the calculated friction torque calculated value τμcal from the value obtained by multiplying the motor current Im by the motor torque constant Kt, the value obtained by multiplying the motor inertia Jm by the motor angular acceleration αm, and the value of the dynamic torque τdyn calculated by the dynamic torque calculation block 26. The collision determination block 31 determines whether a collision has occurred based on the calculated collision torque τdis. If the collision determination block 31 determines that a collision has occurred, the friction estimation block 27 returns the friction torque calculated value τμcal to the friction estimated filtered value from a predetermined time period. The collision torque τdis is calculated using the friction estimated filtered value from the predetermined time period.

[0127] (Summary of this embodiment)

[0128] The following technology is disclosed through the above description of the present embodiment.

[0129] <Technology 1>

[0130] The robot control method according to this embodiment includes a collision detection method. In a robot driven by a motor via a speed reducer, the collision detection method detects an external force by subtracting the dynamic torque calculated through inverse dynamics calculations of the robot and a calculated friction torque value from the torque output from the motor to the speed reducer. If the detected external force value exceeds a predetermined threshold, the robot is determined to have collided. If the robot is determined not to have collided, a friction estimation filter value obtained by smoothing the friction torque estimated value used in setting the calculated friction torque value is updated based on a friction estimation filter value from a predetermined time. The friction estimation filter values ​​before and after the update are stored as friction estimation stored values. Furthermore, if a collision is determined to have occurred, the friction estimation filter value is returned to the friction estimation stored value before the update.

[0131] Thus, the robot control method according to this embodiment can calculate the friction torque calculation value with high accuracy by suppressing errors caused by temperature changes and individual differences in motors by updating the friction estimation filter value at predetermined times using the friction torque estimation value calculated in real time.

[0132] Furthermore, the robot control method can prevent an error in the friction torque calculation value due to the influence of the collision by returning the friction estimation filter value to the friction estimation stored value before the update when the collision occurs.

[0133] Thus, the robot control method can calculate the collision torque with high accuracy, thereby improving the sensitivity of the robot's collision detection.

[0134] <Technology 2>

[0135] The robot control method according to technique 1 performs smoothing processing when the collision detection threshold used for deriving the detection value of the external force is smaller than a predetermined friction estimation threshold.

[0136] Therefore, the robot control method according to the present embodiment can detect relatively weak collisions, and thus can improve the robot's collision detection sensitivity.

[0137] <Technology 3>

[0138] In the robot control method described in Technique 1 or Technique 2, the smoothing process is a process of adding weights to the friction torque estimate value estimated a predetermined time ago and the currently estimated friction torque estimate value.

[0139] Thus, the robot control method according to this embodiment can calculate an estimated friction torque value that takes into account previously calculated friction torque estimates. This prevents a large error in the calculated friction torque value from occurring when a collision actually occurs but the collision torque is less than the collision detection threshold, preventing the collision from being detected.

[0140] <Technique 4>

[0141] In the robot control method described in any one of Techniques 1 to 3, the friction torque estimate value is calculated at every predetermined time.

[0142] Thus, the robot control method according to this embodiment can calculate the friction torque estimate value in real time. Therefore, the robot control method can prevent errors in the friction torque calculation value caused by temperature changes or individual differences in the motors.

[0143] <Technique 5>

[0144] The robot control method according to any one of techniques 1 to 4 smoothes the friction torque estimate value for each motor speed to calculate a friction estimate filter value.

[0145] Thus, the robot control method according to the present embodiment can calculate the collision torque using the friction estimation filter value for each motor speed value.

[0146] While the embodiments have been described above with reference to the accompanying drawings, the present disclosure is not limited to these embodiments. It is apparent that those skilled in the art will be able to devise various variations, modifications, substitutions, additions, deletions, and equivalents within the scope of the claims, and will understand that these also fall within the technical scope of the present disclosure. Furthermore, the various structural elements in the embodiments described above may be arbitrarily combined without departing from the spirit of the invention.

[0147] Industrial applicability

[0148] The technology disclosed herein is useful as a robot control method for improving the robot's collision detection sensitivity.

[0149] -Explanation of symbols-

[0150] 6 Position control block

[0151] 10 Speed ​​control block

[0152] 18 Torque control block

[0153] 21 Direction determination

[0154] 26 Dynamic torque calculation block

[0155] 30 Collision torque estimation block

[0156] 31 collision detection block

[0157] 99 Motor rotation angle detector

[0158] W1, W2, W3, W4 charts

[0159] R1, R3, and R4 areas.

Claims

1. A robot control method having a collision detection method, In a robot driven by a motor via a speed reducer, The collision detection method detects an external force by subtracting the dynamic torque and friction torque calculated by inverse dynamics calculation of the robot from the torque output by the motor to the reducer. If the detected value of the external force is higher than a predetermined threshold, it is determined that the robot has collided. When it is determined that the collision has not occurred, the friction estimation filter value obtained by smoothing the friction torque estimation value used in setting the friction torque calculation value is updated based on the friction estimation filter value before a given time, and the friction estimation filter values ​​before and after the update are saved as friction estimation saved values.

2. The robot control method according to claim 1, wherein: When it is determined that a collision has occurred, the friction estimation filter value is returned to the friction estimation stored value before update.

3. The robot control method according to claim 1 or 2, wherein: The smoothing process is performed when a collision detection threshold used in deriving the detection value of the external force is smaller than a predetermined friction estimation threshold.

4. The robot control method according to claim 1 or 2, wherein: The smoothing process is a process of adding weights to the friction torque estimate value estimated a predetermined time ago and the friction torque estimate value estimated currently.

5. The robot control method according to claim 1 or 2, wherein: The friction torque estimation value is calculated at every predetermined time period.

6. The robot control method according to claim 1 or 2, wherein: The friction torque estimation value is smoothed for each motor speed to calculate the friction estimation filter value.

Images