[go: up one dir, main page]

WO2017047009A1 - ロボットの衝突検出方法 - Google Patents

ロボットの衝突検出方法 Download PDF

Info

Publication number
WO2017047009A1
WO2017047009A1 PCT/JP2016/003855 JP2016003855W WO2017047009A1 WO 2017047009 A1 WO2017047009 A1 WO 2017047009A1 JP 2016003855 W JP2016003855 W JP 2016003855W WO 2017047009 A1 WO2017047009 A1 WO 2017047009A1
Authority
WO
WIPO (PCT)
Prior art keywords
value
collision
threshold
torque
maximum value
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Ceased
Application number
PCT/JP2016/003855
Other languages
English (en)
French (fr)
Inventor
中田　広之
敦実 橋本
良祐 山本
保義 本内
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Panasonic Intellectual Property Management Co Ltd
Original Assignee
Panasonic Intellectual Property Management Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Panasonic Intellectual Property Management Co Ltd filed Critical Panasonic Intellectual Property Management Co Ltd
Priority to JP2017540469A priority Critical patent/JP6706741B2/ja
Priority to EP16845887.5A priority patent/EP3351356B1/en
Priority to US15/580,832 priority patent/US10690558B2/en
Priority to CN201680041206.6A priority patent/CN107848126B/zh
Publication of WO2017047009A1 publication Critical patent/WO2017047009A1/ja
Anticipated expiration legal-status Critical
Ceased legal-status Critical Current

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1674—Programme controls characterised by safety, monitoring, diagnostic
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01L—MEASURING FORCE, STRESS, TORQUE, WORK, MECHANICAL POWER, MECHANICAL EFFICIENCY, OR FLUID PRESSURE
    • G01L5/00—Apparatus for, or methods of, measuring force, work, mechanical power, or torque, specially adapted for specific purposes
    • G01L5/0052—Apparatus for, or methods of, measuring force, work, mechanical power, or torque, specially adapted for specific purposes measuring forces due to impact
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J19/00—Accessories fitted to manipulators, e.g. for monitoring, for viewing; Safety devices combined with or specially adapted for use in connection with manipulators
    • B25J19/02—Sensing devices
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01L—MEASURING FORCE, STRESS, TORQUE, WORK, MECHANICAL POWER, MECHANICAL EFFICIENCY, OR FLUID PRESSURE
    • G01L3/00—Measuring torque, work, mechanical power, or mechanical efficiency, in general
    • G01L3/24—Devices for determining the value of power, e.g. by measuring and simultaneously multiplying the values of torque and revolutions per unit of time, by multiplying the values of tractive or propulsive force and velocity
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01L—MEASURING FORCE, STRESS, TORQUE, WORK, MECHANICAL POWER, MECHANICAL EFFICIENCY, OR FLUID PRESSURE
    • G01L5/00—Apparatus for, or methods of, measuring force, work, mechanical power, or torque, specially adapted for specific purposes
    • G01L5/22—Apparatus for, or methods of, measuring force, work, mechanical power, or torque, specially adapted for specific purposes for measuring the force applied to control members, e.g. control members of vehicles, triggers
    • G01L5/226—Apparatus for, or methods of, measuring force, work, mechanical power, or torque, specially adapted for specific purposes for measuring the force applied to control members, e.g. control members of vehicles, triggers to manipulators, e.g. the force due to gripping
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B2219/00—Program-control systems
    • G05B2219/30—Nc systems
    • G05B2219/40—Robotics, robotics mapping to robotics vision
    • G05B2219/40317—For collision avoidance and detection
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y10—TECHNICAL SUBJECTS COVERED BY FORMER USPC
    • Y10S—TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y10S901/00—Robots
    • Y10S901/46—Sensing device

Definitions

  • the present invention relates to a collision detection method for a robot driven by a motor via a reduction gear.
  • An example of a method for obtaining the collision force at the time of collision without using a collision sensor is a dynamics calculation method (see Non-Patent Document 1).
  • a reduction gear output torque is obtained by subtracting the inertia of the motor and reduction gear and the torque lost due to friction from the torque generated by the driving current of the motor.
  • the collision force is obtained by subtracting the robot dynamic torque obtained from the inverse dynamics calculation of the robot from the obtained reduction gear output torque.
  • FIG. 1 is a block diagram showing a conventional configuration relating to position control of a vertically articulated robot, and shows an outline of internal configurations of a robot mechanism 161 and a robot control device 162.
  • the robot mechanism 161 has a plurality of arms and joint axes, and the robot arms and joint axes are driven using a reduction gear provided in the vicinity of each joint axis.
  • a main control unit 64 provided in the robot control device 162 follows each movement axis of the robot mechanism 161 instructed by the operation / teaching unit 63 and stored in each axis (for example, 6 Axis) position command ⁇ com ( ⁇ com1 to ⁇ com6) is output. Then, the servo control units 165 (1651 to 1656) of the robot axes control the motors 66 (661 to 666) in the robot mechanism 161 so as to follow the position commands ⁇ com ( ⁇ com1 to ⁇ com6), respectively.
  • FIG. 2 is a block diagram showing the servo control unit 165 of FIG. 1 in detail, and adds a sensorless collision detection function that does not use a collision sensor of a dynamic calculation method.
  • the position control block 6 generates a speed loop command ⁇ com from the difference value between the position command ⁇ com and the motor position feedback ⁇ m.
  • the motor position feedback ⁇ m is obtained from an encoder 51 that is a position detector attached to the motor 66.
  • the motor angular acceleration ⁇ m is a differential value of the motor angular velocity ⁇ m.
  • the motor inertia Jm is the sum of the rotor of the motor 66 and the primary side of the speed reducer 53.
  • the dynamic torque ⁇ dyn is the sum of gravity torque, inertial force, centrifugal force, and Coriolis force.
  • the motor generated torque ⁇ m is expressed by the first expression of (Equation 1) when viewed from the drive side of the motor 66, and when viewed from the load side via the speed reducer 53 with respect to the motor 66 ( It is expressed by the second formula of Formula 1).
  • the above dynamic friction torque ⁇ can be calculated by (Equation 2) using the dynamic friction coefficient K ⁇ .
  • the collision torque ⁇ dis on the right side of the second expression of (Equation 1) can be obtained by transforming (Equation 1) into (Equation 3) shown below.
  • Kt ⁇ Im ⁇ Jm ⁇ ⁇ m ⁇ D ⁇ ⁇ m ⁇ K ⁇ ⁇ sgn is a torque output from the motor 66 to the speed reducer 53.
  • Equation 3 (Equation 3) is expressed as a torque estimation block 30.
  • the estimated dynamic torque value ⁇ dyno is obtained in the torque calculation block 26 by performing reverse dynamics calculation using the motor speed feedback of all axes constituting the robot and the machine parameters of the robot.
  • the torque estimation block 30 obtains the collision torque estimated value ⁇ diso using the dynamic torque estimated value ⁇ dyno, and outputs the collision torque estimated value ⁇ diso to the collision determination block 31.
  • the collision determination block 31 detects a collision according to (Equation 4) using a predetermined collision detection threshold ⁇ th.
  • the collision torque estimated value ⁇ diso does not become “0” even when there is no actual collision due to a parameter error or a model error in dynamic torque calculation. That is, when there is no actual collision, the collision torque estimation error is output as the collision torque estimated value ⁇ diso.
  • the collision torque estimated value ⁇ diso is obtained using the viscous friction coefficient D measured at the normal temperature, the error becomes large. Therefore, there is a possibility that the collision detection threshold ⁇ th needs to be increased in order to prevent erroneous collision detection.
  • Patent Document 1 discloses a method of measuring a temperature and raising a collision detection threshold at a low temperature as described below.
  • FIG. 3 and FIG. 4 show other prior arts, corresponding to FIG. 1 and FIG. 2, respectively, with a configuration in which a temperature sensor is attached to the speed reducer and the collision detection threshold is switched.
  • the temperature sensor 56 is attached to the speed reducer 53.
  • the temperature sensor output value Tc of the temperature sensor 56 is input to the first servo control unit 165.
  • the threshold value switching block 34 compares the temperature sensor output value Tc with a predetermined temperature threshold value Tcth, and uses the collision detection threshold value ⁇ th at normal temperature and the threshold increment d ⁇ th corresponding to low temperature to detect collision according to (Equation 5).
  • the threshold value ⁇ vth is output to the collision determination block 40 (d ⁇ th> 0).
  • the collision detection threshold value ⁇ th at normal temperature is obtained in advance by actually operating the speed reducer 53 at normal temperature (for example, 5 ° C. or more).
  • the threshold increment d ⁇ th corresponding to the low temperature is obtained in advance by actually operating at a low temperature (for example, a temperature of less than 5 ° C.).
  • the collision determination block 40 detects a collision according to (Equation 6) shown below using the collision detection threshold value ⁇ vth calculated in (Equation 5).
  • the robot collision is detected by the following method.
  • the robot includes a motor, a speed reducer connected to the motor, an encoder that detects rotation of the motor, a temperature sensor built in the encoder, and an object driven by the motor via the speed reducer.
  • the external force torque due to the collision is estimated as the estimated value of the collision torque. If the estimated collision torque is greater than a predetermined collision detection threshold, it is determined that the robot has received an external force.
  • the predetermined collision detection threshold is set to the first value.
  • the maximum value of the collision torque estimation value in the period from the second time point to the first time point is the first maximum value determination. If it is smaller than the threshold, the predetermined collision detection threshold is set to a second value smaller than the first value at the first time point. When the maximum value of the estimated collision torque in the period is equal to or greater than the first maximum value determination threshold, the predetermined collision detection threshold is set to the first value at the first time point.
  • This collision detection method can reduce the occurrence of erroneous detection of a collision even when the collision detection threshold is switched using a temperature sensor with a built-in encoder.
  • FIG. 1 is a block diagram showing a conventional configuration relating to position control of a robot.
  • FIG. 2 is a block diagram showing a collision detection method in the configuration shown in FIG.
  • FIG. 3 is a block diagram showing another conventional configuration relating to the position control of the robot.
  • FIG. 4 is a block diagram showing a collision detection method in the configuration shown in FIG.
  • FIG. 5 is a block diagram showing still another conventional configuration relating to position control of a vertical articulated robot.
  • FIG. 6 is a diagram showing a positional relationship when the motor with an encoder is directly attached to the speed reducer.
  • FIG. 7 is a diagram showing a positional relationship when a belt is interposed between the speed reducer and the motor with the encoder.
  • FIG. 8 is a schematic configuration diagram of the robot in the embodiment.
  • FIG. 9 is a block diagram showing a collision detection method in the embodiment.
  • FIG. 10 is a block diagram showing details of the waveform analysis block in the embodiment.
  • FIG. 11 is a flowchart showing processing of the maximum value extraction block in the embodiment.
  • FIG. 12 is a diagram illustrating waveforms in the collision detection method according to the embodiment.
  • FIG. 13 is a flowchart showing the process of the switching permission determination block in the embodiment.
  • FIG. 14 is a flowchart showing the processing of the threshold switching block in the embodiment.
  • FIG. 15 is a diagram illustrating an operation in the collision detection method according to the embodiment.
  • FIG. 16 is a diagram illustrating another operation in the collision detection method according to the embodiment.
  • FIG. 17 is a diagram illustrating still another operation in the collision detection method according to the embodiment.
  • FIG. 18 is a diagram showing the operation shown in FIG.
  • FIG. 19 is a diagram showing still another operation in the collision detection method in the embodiment.
  • FIG. 20 shows the operation shown in FIG.
  • a temperature sensor is built in an encoder which is a motor position detector built in or attached to a motor, and temperature can be read by serial communication data as well as position information.
  • FIG. 5 shows a configuration in which the encoder 51 (511 to 516) shown in FIG. 1 is replaced with an encoder 52 (521 to 526) each incorporating a temperature sensor 56 (561 to 566).
  • the motors 66 (661 to 666) drive the joint shafts 67A (671A to 676A), which are objects, via the speed reducers 53 (531 to 536), respectively. Since the temperature sensor output value Tc of the temperature sensor 56 built in the encoder 52 can be read by the same serial communication as the position information, the cost does not increase and the number of cables in the robot does not increase.
  • FIG. 6 shows the positional relationship when the motor 66 with the encoder 51 is directly attached to the speed reducer 53.
  • FIG. 7 shows the positional relationship when a belt 68 is interposed between the motor 66 with the encoder 51 and the speed reducer 53.
  • the temperature of the speed reducer 53 has increased to the same extent as the internal temperature of the encoder 51.
  • the belt 68 or the like is interposed between the motor 66 and the speed reducer 53 as shown in FIG. 7, the temperature of the speed reducer 53 is not necessarily increased to the same extent as the internal temperature of the encoder 52. .
  • the collision detection threshold value ⁇ vth is calculated according to (Equation 5), and the threshold value increment d ⁇ th corresponding to the low temperature is eliminated, thereby detecting the threshold value.
  • the ⁇ vth is returned to the collision detection threshold ⁇ th corresponding to the normal temperature that does not consider the low temperature, which is the original value.
  • FIG. 8 is a schematic configuration diagram of a robot 1001 that is a vertical articulated six-axis robot.
  • the robot 1001 includes a robot mechanism 61 and a robot control device 62.
  • the robot mechanism 61 has a plurality of arms 67 (and a plurality of joint shafts 67A (671A to 676A)), and is driven using a speed reducer provided in the vicinity of each joint shaft.
  • FIG. 9 is a block diagram showing a collision detection method in the embodiment.
  • FIG. 9 further includes a waveform analysis block 57 based on the dynamic calculation method shown in FIG.
  • FIG. 10 shows details of the waveform analysis block 57.
  • the unit time is defined as an interval for performing a waveform analysis of the estimated collision torque value. In the embodiment, this unit time is, for example, 1 minute. Note that the unit time refers to the time during which the robot 1001 is operating, excluding the time during which the robot 1001 is stopped.
  • the peak hold block 74 calculates a peak hold value ⁇ dP which is a value obtained by holding a peak within a unit time of the absolute value of the collision torque estimated value ⁇ diso.
  • FIG. 11 is a flowchart showing a method for calculating the peak hold value ⁇ dP.
  • the main control unit 64 performs the process of FIG. 11 a predetermined number of times for each calculation cycle within the unit time of the estimated collision torque value ⁇ diso. The calculation cycle is several msec in the embodiment.
  • the main control unit 64 calculates the peak hold value ⁇ dP by performing the operation shown in FIG. 11 a predetermined number of times num within a unit time and outputs it.
  • the peak hold block 74 determines whether or not the peak hold frequency i is 0 (step 11-1). If the number of times i is 0 in step 11-1, that is, the first time (“Yes” in step 11-1), the peak hold block 74 clears the peak hold value ⁇ dP to 0 (step 11-1A). If the number of times i is not 0 in step 11-1, that is, if it is not the first time (“No” in step 11-1), the main control unit 64 keeps the peak hold value ⁇ dP as it is.
  • the peak hold block 74 determines whether or not the absolute value of the angular velocity ⁇ mMAX having the maximum absolute value among the angular velocities ⁇ m of the joint shaft 671A and the angular velocities ⁇ m of the other joint shafts 672A to 676A is greater than the angular velocity threshold ⁇ mth ( Step 11-2).
  • the angular velocity threshold value ⁇ mth is a threshold value for determining whether or not to perform peak hold.
  • step 11-2 The reason for providing the condition of step 11-2 is that when the robot 1001 is stopped due to a stop operation by the user, waiting for sensor signal input, or the like, the error of the collision torque estimated value ⁇ diso caused by the high viscous friction coefficient D This is because no problem occurs.
  • the angular velocity ⁇ m of the motor 66 is monitored, but other parameters that can confirm that the robot 1001 is operating may be monitored instead of the angular velocity ⁇ m.
  • the robot operation may be determined when an operation command is executed in the program of the robot 1001 or when a position command is generated.
  • the peak hold block 74 determines whether or not the absolute value of the collision torque estimated value ⁇ diso obtained by the torque estimation block 30 using the dynamic torque estimated value ⁇ dyno is larger than the peak hold value ⁇ dP (step 11-3). If the absolute value of the collision torque estimated value ⁇ dis is larger than the peak hold value ⁇ dP in Step 11-3 (“Yes” in Step 11-3), the peak hold block 74 converts the peak hold value ⁇ dP to the collision torque estimated value ⁇ diso. Update to an absolute value. If the absolute value of the collision torque estimated value ⁇ diso is not larger than the peak hold value ⁇ dP in Step 11-3 (“No” in Step 11-3), the peak hold block 74 leaves the peak hold value ⁇ dP as it is.
  • the peak hold block 74 determines whether or not the peak hold count i reaches the end count num and is equal to or greater than the end count num (step 11-4). In step 11-4, if the peak hold count i is equal to or greater than the end count num (“Yes” in step 11-4), the peak hold block 74 outputs the peak hold value ⁇ dP and clears the count i to 0. In this manner (step 11-4A), the process ends in step 11-4B. In step 11-4, if the peak hold count i is not equal to or greater than the end count num (“No” in step 11-4), the peak hold block 74 ends the processing in step 11-4B while leaving the count i unchanged.
  • the peak hold value ⁇ dP per unit time output from the peak hold block 74 is stored in the data storage block 75.
  • the peak hold value ⁇ dP for the x-th unit time (for example, the xth minute if the unit time is 1 minute) as the collision torque estimated value ⁇ diso is defined as the peak hold value ⁇ dP [x]. If N peak hold values ⁇ dP are currently stored in the x-th unit time, the data storage block 75 stores peak hold values ⁇ dP [x ⁇ N + 1], ⁇ dP [x ⁇ N + 2],. x].
  • FIG. 12 shows the temperature sensor output value Tc of the temperature sensor 56 built in the encoder 52 and the x-th peak hold value ⁇ dP [x].
  • the vertical axis indicates the temperature sensor output value Tc and the peak hold value ⁇ dP [x], which is a collision torque
  • the horizontal axis indicates the number of times x per unit time.
  • the collision torque and the threshold value on the vertical axis are indicated by a maximum torque ratio (%) which is a ratio to the maximum torque.
  • a collision does not actually occur, and the collision torque estimated value ⁇ dP [x] decreases as the temperature rises, that is, the viscous friction coefficient D decreases. This shows a case where the error of the estimated collision torque ⁇ dP [x] is reduced.
  • the slope calculation block 77 in FIG. 10 calculates the slope ⁇ dPA of the approximate line 83 from the collision torque estimated value ⁇ dP [x ⁇ N + 1] to ⁇ dP [x].
  • a slope ⁇ dPA is obtained by calculating the slope of the approximate straight line 83.
  • FIG. 13 is a flowchart showing processing executed by the switching permission determination block 78 of FIG. Next, the switching permission determination block 78 executes the processing shown in the flowchart of FIG.
  • the switching permission determination block 78 first sets (clears) the switching permission value CE to a number other than “1”, for example, “0” (step 13-0).
  • the switching permission determination block 78 determines that the maximum value ⁇ dPP that is the maximum value of the collision torque in the period Du from a predetermined time before the collision detection threshold switching time to the collision detection threshold switching time is the maximum value determination threshold ⁇ dPPthA. It is determined whether it is smaller (step 13-1). If the maximum value ⁇ dPP is smaller than the maximum value determination threshold ⁇ dPPthA in step 13-1 (“Yes” in step 13-1), the switching permission determination block 78 sets the switching permission value CE to “1” (step 13- 1A) Finish the process.
  • the switching permission determination block 78 determines whether the maximum value ⁇ dPP is smaller than the maximum value determination threshold ⁇ dPPthB. Is determined (step 13-2). If the maximum value ⁇ dPP is not smaller than the maximum value determination threshold value ⁇ dPPthB in step 13-2 (“No” in step 13-2), the process ends. However, the maximum value determination threshold value ⁇ dPPthA, the maximum value determination threshold value ⁇ dPPthB, and the collision detection threshold value ⁇ th at normal temperature are set so as to satisfy the relationship of ⁇ dPPthA ⁇ dPPthB ⁇ th.
  • the switching permission determination block 78 determines that the inclination ⁇ dPA of the approximate line 83 is greater than the inclination determination threshold value ⁇ dPAth. It is determined whether or not it is smaller (step 13-3). If it is smaller than the inclination determination threshold ⁇ dPAth in Step 13-3 (“Yes” in Step 13-3), the switching permission determination block 78 sets the switching permission value CE to “1” and ends the process. When it is not smaller than the inclination determination threshold ⁇ dPAth in Step 13-3 (“No” in Step 13-3), the switching permission determination block 78 does nothing and ends the process.
  • the threshold switching block 41 outputs the collision detection threshold ⁇ vth to the collision determination block according to (Equation 7) using the collision detection threshold ⁇ th corresponding to normal temperature and the threshold increment d ⁇ th corresponding to low temperature.
  • the temperature threshold Tcth is the temperature of the speed reducer 53 when the collision detection threshold ⁇ th and the threshold increment d ⁇ th are determined ( For example, it may be set to 10 ° C. in consideration of a margin.
  • FIG. 14 is a flowchart showing the processing of (Equation 7) executed by the threshold switching block 41.
  • the threshold switching block 41 determines whether or not the temperature sensor output value Tc is greater than or equal to a predetermined temperature threshold Tcth (step 14-1). If the temperature sensor output value Tc is not equal to or greater than the predetermined temperature threshold Tcth in Step 14-1 (“No” in Step 14-1), the collision detection threshold ⁇ th and the threshold increment d ⁇ th are added as the collision detection threshold ⁇ vth. The obtained ( ⁇ th + d ⁇ th) is output, the permission value CE is set to “0”, and the process ends.
  • the threshold value switching block 41 determines whether or not the switching permission value CE is “1”. (Step 14-2). When the switching permission value CE is “1” in Step 14-2 (“Yes” in Step 14-2), the threshold switching block 41 sets the collision detection threshold ⁇ th corresponding to the normal temperature as the collision detection threshold ⁇ vth. (Step 14-2), the permission value CE is left as it is, and the process is terminated.
  • the vertical axis represents the temperature sensor output value Tc and the x-th peak hold value ⁇ dP [x] of the collision torque
  • the horizontal axis represents the number x of unit times.
  • the collision torque and the threshold on the vertical axis are indicated by a maximum torque ratio (%) that is a ratio to the maximum torque.
  • the collision torque estimated value ⁇ diso (collision torque estimation error) becomes smaller as the temperature Tc increases.
  • the maximum value ⁇ dPP is smaller than the maximum value determination threshold ⁇ dPPthA and sufficiently smaller than the value ⁇ th corresponding to the normal temperature of the collision detection threshold ⁇ vth. In this way, with the threshold value [%] in FIG. 15, even if the collision detection threshold value ⁇ vth is reduced, it is difficult for erroneous collision detection to occur.
  • the reverse dynamics of the robot 1001 is determined from the torque output from the motor 66 to the speed reducer 53.
  • the dynamic torque obtained by the calculation the external force torque due to the collision is estimated as the collision torque estimated value ⁇ diso. If the collision torque estimated value ⁇ diso is larger than a predetermined collision detection threshold ⁇ th, it is determined that the robot 1001 has received an external force.
  • the collision detection threshold value ⁇ th of the estimated collision torque value ⁇ diso is switched to a smaller value.
  • the maximum value ⁇ dPP of the collision torque estimated value ⁇ diso from a predetermined time before the switching point of the collision detection threshold ⁇ th is the collision torque estimated value. If the collision detection threshold value ⁇ th is smaller than the maximum value determination threshold value ⁇ dPPthA smaller than the value after the collision detection threshold value ⁇ th is switched, the switching of the collision detection threshold value ⁇ th is permitted.
  • FIG. 16 shows a temperature sensor output value Tc and a peak hold value ⁇ dP in another operation of the collision detection method in the embodiment.
  • the vertical axis indicates the temperature sensor output value Tc and the x-th peak hold value ⁇ dP [x] of the collision torque
  • the horizontal axis indicates the number of times x per unit time.
  • the collision torque and the threshold on the vertical axis are indicated by a maximum torque ratio (%) which is a ratio to the maximum torque.
  • the temperature rise of the speed reducer 53 is lower than the rise of the temperature sensor output value Tc and the viscous friction coefficient D is not sufficiently lowered as compared with FIG. 15.
  • FIG. 17 shows a temperature sensor output value Tc and a peak hold value ⁇ dP in still another operation of the collision detection method in the embodiment.
  • the vertical axis represents the temperature sensor output value Tc and the x-th peak hold value ⁇ dP [x] of the collision torque
  • the horizontal axis represents the number of times x per unit time.
  • the collision torque and the threshold on the vertical axis are indicated by a maximum torque ratio (%) which is a ratio to the maximum torque.
  • the inclination determination threshold ⁇ dPAth is set to 0.
  • the collision torque estimated value ⁇ dis (collision torque estimation error) tends to increase, that is, the peak hold value ⁇ dP [x] tends to increase, in order to prevent erroneous collision detection, in step 13-3 in FIG.
  • the determination is “No”, the switching permission value CE is not set to “1”, and the collision detection threshold ⁇ vth is not lowered from the value corresponding to the low temperature (70%).
  • the collision torque estimation value ⁇ dis (collision torque estimation error) tends to increase, that is, the peak hold value ⁇ dP [x] tends to increase
  • FIG. 19 shows a temperature sensor output value Tc and a peak hold value ⁇ dP in still another operation of the collision detection method in the embodiment.
  • the vertical axis represents the temperature sensor output value Tc and the x-th peak hold value ⁇ dP [x] of the collision torque
  • the horizontal axis represents the number of times x per unit time.
  • the collision torque and the threshold on the vertical axis are indicated by a maximum torque ratio (%) that is a ratio to the maximum torque.
  • the operation shown in FIG. 19 shows the operation in the case where the temperature increase of the speed reducer 53 is lower than the temperature sensor output value Tc of the temperature sensor 56 and the viscous friction coefficient D is not reduced as compared with FIG.
  • the maximum value determination threshold ⁇ dPPthB that is larger than the maximum value determination threshold ⁇ dPPthA and smaller than the value after switching of the collision detection threshold ⁇ vth is provided.
  • the maximum value ⁇ dPP of the collision torque estimation value ⁇ diso from a predetermined time before the switching time of the collision detection threshold value ⁇ vth of the collision torque estimation value ⁇ dis is greater than or equal to the maximum value determination threshold value ⁇ dPPthA and less than the maximum value determination threshold value ⁇ dPPthB.
  • the robot 1001 includes the motor 66, the speed reducer 53 connected to the motor 66, the encoder 51 that detects the rotation of the motor 66, the temperature sensor 56 built in the encoder 51, and the speed reducer 53. And an object (arm 67) driven by a motor 66.
  • the external force torque due to the collision is estimated as a collision torque estimated value ⁇ diso ( ⁇ dP). If the collision torque estimated value ⁇ diso ( ⁇ dP) is larger than a predetermined collision detection threshold ⁇ vth, it is determined that the robot 1001 has received an external force.
  • a maximum value determination threshold value ⁇ dPPthB that is larger than the maximum value determination threshold value ⁇ dPPthA and smaller than the value ⁇ th may be set.
  • the slope ⁇ dPA of the approximate line 83 obtained by approximating the time transition of the collision torque estimated value ⁇ diso ( ⁇ dP) in the period Du may be obtained.
  • the predetermined collision detection threshold value ⁇ vth is set to the value ⁇ th.
  • the predetermined collision detection threshold value ⁇ vth is set to a value ( ⁇ th + d ⁇ th).
  • ⁇ dPP of the collision torque estimated value ⁇ diso ( ⁇ dP) in the period Du is equal to or greater than the maximum value determination threshold ⁇ dPPthB
  • ⁇ vth may be a value ( ⁇ th + d ⁇ th).
  • the collision detection threshold value ⁇ vth is switched using the temperature sensor 56 built in the encoder 52 by the collision detection method of the robot 1001 in the embodiment, the occurrence of erroneous detection of collision can be reduced.
  • the robot collision detection method of the present invention uses a temperature sensor with a built-in motor encoder without switching the temperature sensor to a speed reducer that causes an increase in cost and reliability in the collision detection threshold switching by the temperature sensor. Even in this case, it is possible to reduce the occurrence of erroneous collision detection, which is industrially useful.

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Chemical & Material Sciences (AREA)
  • Combustion & Propulsion (AREA)
  • Manipulator (AREA)

Abstract

ロボットの衝突は以下の方法で検出する。ロボットは、モータと、モータに接続された減速機と、モータの回転を検出するエンコーダと、エンコーダに内蔵された温度センサと、減速機を介してモータにより駆動される対象物とを有する。モータが減速機に出力するトルクから、ロボットの逆動力学演算で求めた動力学トルクを差し引くことにより、衝突による外力トルクを衝突トルク推定値として推定する。衝突トルク推定値が所定の衝突検出閾値より大きければ、ロボットが外力を受けたと判断する。温度センサで検出された温度が所定の温度閾値未満である場合に、所定の衝突検出閾値を第１の値にする。検出された温度が所定の温度閾値以上である第１の時点より所定時間だけ前の第２の時点から第１の時点までの期間での衝突トルク推定値の最大値が第１の最大値判定閾値より小さい場合に、第１の時点において所定の衝突検出閾値を第１の値より小さい第２の値にする。上記期間での衝突トルク推定値の最大値が第１の最大値判定閾値以上である場合に、第１の時点において所定の衝突検出閾値を第１の値にする。

Description

ロボットの衝突検出方法
　本発明は、減速機を介してモータにより駆動されるロボットの衝突検出方法に関する。
　近年、ロボットにおいて、衝突時の安全性向上や破壊による損失防止のために、衝突検出の高精度化が求められている。しかしながら、高精度な衝突センサを用いることはコストが増大し、さらに重量負荷としてのその衝突センサを振り回すことになるので、ロボットの高速化や省エネルギーに反することになる。そこで、衝突センサを用いないで、衝突力を高精度に求めることが望まれている。
　衝突の際の衝突力を、衝突センサを用いないで求める方法の一例としては動力学演算方式が挙げられる（非特許文献１参照）。動力学演算方式では、モータの駆動電流で発生したトルクからモータ及び減速機のイナーシャと摩擦で損失するトルクとを差し引いた減速機出力トルクを求める。求めた減速機出力トルクより、ロボットの逆動力学演算から求めたロボットの動力学トルクを差し引いて衝突力を求める。
　図１は、垂直多関節のロボットの位置制御に関する従来の構成を示すブロック図であり、ロボット機構１６１とロボット制御装置１６２の内部構成の概略を示す。ここで、ロボット機構１６１は複数のアームおよび関節軸を有しており、各関節軸の近傍に設けられた減速機を用いてロボットのアームおよび関節軸が駆動される。
　図１において、ロボット制御装置１６２の内部に設けられたメイン制御部６４は、操作・教示部６３で指示され記憶されたロボット機構１６１のアーム等の移動の軌跡に従い、ロボットの各軸（例えば６軸）の位置指令θｃｏｍ（θｃｏｍ１～θｃｏｍ６）を出力する。そして、位置指令θｃｏｍ（θｃｏｍ１～θｃｏｍ６）にそれぞれ追従するように、ロボットの各軸のサーボ制御部１６５（１６５１～１６５６）がロボット機構１６１内のモータ６６（６６１～６６６）をそれぞれ制御する。
　図２は、図１のサーボ制御部１６５を詳細に示したブロック線図であり、動力学演算方式の衝突センサを用いないセンサレス衝突検出機能を加えている。
　図２において、位置制御ブロック６は、位置指令θｃｏｍとモータ位置フィードバックθｍとの差分値から、速度ループ指令ωｃｏｍを生成する。モータ位置フィードバックθｍはモータ６６に取り付けられた位置検出器であるエンコーダ５１から得られる。
　図２の速度制御ブロック１０は、モータ位置フィードバックθｍを微分して得られるモータ速度フィードバックωｍと速度ループ指令ωｃｏｍとの差分値からモータ電流指令Ｉｍを生成する。
　図２のブロック１８はモータ６６と外力を示す。減速機５３が剛体であると仮定すると、モータトルク定数Ｋｔと、モータ電流Ｉｍと、モータ角加速度αｍと、モータ角速度ωｍと、モータイナーシャＪｍと、粘性摩擦係数Ｄと、動摩擦トルクτμと、動力学トルクτｄｙｎと、衝突トルクτｄｉｓとにより、モータ発生トルクτｍは（数１）で表される。
Figure JPOXMLDOC01-appb-M000001
　なお、モータ角加速度αｍはモータ角速度ωｍの微分値である。モータイナーシャＪｍは、モータ６６のロータと減速機５３の一次側の和である。動力学トルクτｄｙｎは、重力トルクと慣性力と遠心力とコリオリ力の和である。
　具体的には、モータ発生トルクτｍはモータ６６の駆動側から見れば（数１）のうちの第１の式で表され、モータ６６に対して減速機５３を介した負荷側から見れば（数１）の第２の式で表される。
　上記の動摩擦トルクτμは、動摩擦係数Ｋμにより（数２）で計算できる。
Figure JPOXMLDOC01-appb-M000002
　また、（数１）の第２の式の右辺にある衝突トルクτｄｉｓは、（数１）より、以下に示す（数３）に変形して求めることが出来る。
Figure JPOXMLDOC01-appb-M000003
　なお、（数３）において、Ｋｔ×Ｉｍ－Ｊｍ×αｍ－Ｄ×ωｍ－Ｋμ×ｓｇｎはモータ６６が減速機５３に出力するトルクである。
　図２において、（数３）はトルク推定ブロック３０として表される。
　トルク推定ブロック３０において、動力学トルク推定値τｄｙｎｏは、トルク演算ブロック２６において、ロボットを構成する全軸のモータ速度フィードバックとロボットの機械パラメータを用いて逆動力学演算を実行することで求められる。トルク推定ブロック３０は、動力学トルク推定値τｄｙｎｏを用いて衝突トルク推定値τｄｉｓｏをもとめ、衝突トルク推定値τｄｉｓｏを衝突判定ブロック３１へ出力する。
　衝突判定ブロック３１は、所定の衝突検出閾値τｔｈを用いて、（数４）に従い衝突を検出する。
Figure JPOXMLDOC01-appb-M000004
　すなわち、（数４）が成り立つ場合にメイン制御部６４は衝突が発生したと判定し、（数４）が成り立たない場合にはメイン制御部６４は衝突は発生していないと判定する。
　図２の衝突トルク推定値τｄｉｓｏを求めるトルク推定ブロック３０において、パラメータ誤差や動力学トルク演算のモデル誤差等により、実際に衝突していない場合でも衝突トルク推定値τｄｉｓｏは「０」にはならない。つまり、実際に衝突していない場合は、衝突トルクの推定誤差が衝突トルク推定値τｄｉｓｏとして出力される。
　パラメータ誤差の中でも、特に粘性摩擦係数Ｄは約５℃以下の低温になるとグリスの粘性が上昇して大きな誤差が発生する。通常温度で測定した粘性摩擦係数Ｄを用いて衝突トルク推定値τｄｉｓｏを求めると誤差が大きくなるので、衝突誤検出を防ぐために衝突検出閾値τｔｈを大きくしなければならない可能性がある。
　特許文献１では、以下に説明するように温度を測定し、低温時に衝突検出閾値を引上げる方法が示されている。
　図３、図４は、他の従来技術を示し、それぞれ図１、図２に対応し、減速機に温度センサを取り付け、衝突検出閾値を切換える構成が加えられている。
　図３において、温度センサ５６が減速機５３に取り付けられている。温度センサ５６の温度センサ出力値Ｔｃを、第１のサーボ制御部１６５に入力している。
　図４において、閾値切換ブロック３４では、温度センサ出力値Ｔｃを所定の温度閾値Ｔｃｔｈと比較し、常温時の衝突検出閾値τｔｈと低温に対応した閾値増分ｄτｔｈを用いて（数５）に従って衝突検出閾値τｖｔｈを衝突判定ブロック４０に出力する（ｄτｔｈ＞０）。
Figure JPOXMLDOC01-appb-M000005
　なお、常温時の衝突検出閾値τｔｈは、減速機５３の温度が常温（例えば５℃以上）で実際に動作させ予め求められている。また、低温に対応した閾値増分ｄτｔｈは低温（例えば５℃未満の温度）で実際に動作させて予め求められている。
　衝突判定ブロック４０は、（数５）で演算された衝突検出閾値τｖｔｈを用いて、以下に示す（数６）に従い衝突を検出する。
Figure JPOXMLDOC01-appb-M000006
　すなわち、（数６）が成り立つ場合にメイン制御部６４は衝突が発生したと判定し、（数６）が成り立たない場合にはメイン制御部６４は衝突は発生していないと判定する。
特開平１１－１５５１１号公報
小菅一弘、他一名、"マニピュレータの動的衝突検出"、日本機械学会［Ｎｏ．９９‐９］ロボティクス・メカトロニクス講演会‘９９講演論文集　２Ａ１‐１１‐０３０
　ロボットの衝突は以下の方法で検出する。ロボットは、モータと、モータに接続された減速機と、モータの回転を検出するエンコーダと、エンコーダに内蔵された温度センサと、減速機を介してモータにより駆動される対象物とを有する。モータが減速機に出力するトルクから、ロボットの逆動力学演算で求めた動力学トルクを差し引くことにより、衝突による外力トルクを衝突トルク推定値として推定する。衝突トルク推定値が所定の衝突検出閾値より大きければ、ロボットが外力を受けたと判断する。温度センサで検出された温度が所定の温度閾値未満である場合に、所定の衝突検出閾値を第１の値にする。検出された温度が所定の温度閾値以上である第１の時点より所定時間だけ前の第２の時点から第１の時点までの期間での衝突トルク推定値の最大値が第１の最大値判定閾値より小さい場合に、第１の時点において所定の衝突検出閾値を第１の値より小さい第２の値にする。上記期間での衝突トルク推定値の最大値が第１の最大値判定閾値以上である場合に、第１の時点において所定の衝突検出閾値を第１の値にする。
　この衝突検出方法は、エンコーダ内蔵の温度センサを用いて衝突検出閾値を切換える場合でも、衝突の誤検出の発生を減じることができる。
図１はロボットの位置制御に関する従来の構成を示すブロック図である。 図２は図１に示す構成での衝突検出方法を示すブロック線図である。 図３はロボットの位置制御に関する他の従来の構成を示すブロック図である。 図４は図３に示す構成での衝突検出方法を示すブロック線図である。 図５は垂直多関節ロボットの位置制御に関するさらに他の従来の構成を示すブロック図である。 図６は減速機にエンコーダ付モータが直接取り付けられた場合の位置関係を示す図である。 図７は減速機とエンコーダ付モータとの間にベルトが介在する場合の位置関係を示す図である。 図８は実施の形態におけるロボットの概略構成図である。 図９は実施の形態における衝突検出方法を示すブロック線図である。 図１０は実施の形態における波形分析ブロックの詳細を示すブロック図である。 図１１は実施の形態における最大値抽出ブロックの処理を示すフローチャートである。 図１２は実施の形態における衝突検出方法での波形を示す図である。 図１３は実施の形態における切換え許可判定ブロックの処理を示すフローチャートである。 図１４は実施の形態における閾値切換ブロックの処理を示すフローチャートである。 図１５は実施の形態における衝突検出方法での動作を示す図である。 図１６は実施の形態における衝突検出方法での他の動作を示す図である。 図１７は実施の形態における衝突検出方法でのさらに他の動作を示す図である。 図１８は図１７に示す動作を示す図である。 図１９は実施の形態における衝突検出方法でのさらに他の動作を示す図である。 図２０は図１９に示す動作を示す図である。
　図３に示す構成で減速機５３の温度を測定するためには、温度センサ５６を減速機５３に取り付ける必要があるが、コストが上昇してしまう。また、温度センサ出力値Ｔｃをロボット制御装置１６２内のサーボ制御部１６５に入力するためには、ロボット機構１６１内のケーブルを増やす必要があり、配線処理や断線等の信頼性に問題が生じる可能性がある。
　近年、モータに内蔵または取り付けられているモータの位置検出器であるエンコーダに温度センサが内蔵され、位置情報と同様にシリアル通信データで温度の読取りが可能となっている場合がある。
　モータ動作発熱に起因してエンコーダ内温度が上昇し、約９０℃以上になると、エンコーダを構成する電子部品が誤動作し、正しい位置情報を送信できなくなる。そのため、温度センサを内蔵し、温度が設定値（約９０℃）以上になるとアラーム信号をシリアル信号で出力する。
　図５は、図１に示すエンコーダ５１（５１１～５１６）を、温度センサ５６（５６１～５６６）をそれぞれ内蔵したエンコーダ５２（５２１～５２６）に置き換えた構成を示している。モータ６６（６６１～６６６）は減速機５３（５３１～５３６）を介して対象物である関節軸６７Ａ（６７１Ａ～６７６Ａ）をそれぞれ駆動する。エンコーダ５２に内蔵された温度センサ５６の温度センサ出力値Ｔｃは、位置情報と同じシリアル通信で読み取れるので、コストの上昇やロボット内のケーブルの数は増加しない。
　ただし、この温度はモータ６６のエンコーダ５１の内部の温度であり、減速機の温度ではない。図６は減速機５３にエンコーダ５１が付されたモータ６６が直接取り付けられた場合の位置関係を示す。図７はエンコーダ５１が付されたモータ６６と減速機５３との間にベルト６８が介在する場合の位置関係を示す。図６に示す様に減速機５３がモータ６６と直結されている場合には、エンコーダ５１の内部温度と同程度に減速機５３の温度が上昇していると考えられる。しかし、図７に示す様にモータ６６と減速機５３の間にベルト６８等が介在する場合には、エンコーダ５２の内部温度と同程度に減速機５３の温度が上昇しているとは限らない。
　モータ６６に内蔵または取り付けられているエンコーダ５２の内部温度が所定の温度閾値Ｔｃｔｈを超えた時、（数５）に従って衝突検出閾値τｖｔｈを演算し、低温に対応した閾値増分ｄτｔｈを無くして検出閾値τｖｔｈを元の値である低温時を考慮していない常温に対応する衝突検出閾値τｔｈに戻す。この時、モータ６６の駆動力が伝達される減速機５３の温度上昇が十分でなければ、粘性摩擦係数Ｄが高いことに起因する衝突トルク推定値τｄｉｓｏの誤差が衝突検出閾値τｖｔｈより大きくなり、衝突誤検出が発生する可能性がある。
　（実施の形態）
　図８は、垂直多関節六軸ロボットであるロボット１００１の概略構成図である。ロボット１００１はロボット機構６１とロボット制御装置６２とから構成される。ロボット機構６１は、複数のアーム６７（および複数の関節軸６７Ａ（６７１Ａ～６７６Ａ）を有しており、各関節軸の近傍に設けられた減速機を用いて駆動される。
　図９は、実施の形態における衝突検出方法を示すブロック線図である。図９は、図４で示した動力学演算方式をベースに、波形分析ブロック５７をさらに備える。
　図１０は波形分析ブロック５７の詳細を示す。なお、以下の説明において、単位時間とは衝突トルク推定値の波形分析を行う間隔と定義する。実施の形態においてはこの単位時間は、例えば１分間である。なお、単位時間はロボット１００１が停止している時間は除き、稼動している時間を指す。
　ピークホールドブロック７４は、衝突トルク推定値τｄｉｓｏの絶対値の単位時間内でのピークをホールドして得た値であるピークホールド値τｄＰを算出する。
　複数の関節軸６７Ａ（６７１Ａ～６７６Ａ）のうちの関節軸６７１Ａを駆動するモータ６６１と減速機５３１とエンコーダ５２と温度センサ５６１の動作について説明する。図１１はピークホールド値τｄＰの算出方法を示すフローチャートである。メイン制御部６４は、図１１の処理を衝突トルク推定値τｄｉｓｏの単位時間内において演算周期毎に所定の回数だけ実施する。演算周期は実施の形態では数ｍｓｅｃである。メイン制御部６４は単位時間内に図１１に示す動作を所定の回数ｎｕｍだけ行ってピークホールド値τｄＰを算出して出力する。
　ピークホールドブロック７４は、ピークホールドの回数ｉが０であるか否かを判定する（ステップ１１－１）。ステップ１１－１において回数ｉが０すなわち初回であれば（ステップ１１－１の「Ｙｅｓ」）、ピークホールドブロック７４はピークホールド値τｄＰをクリアして０にする（ステップ１１－１Ａ）。メイン制御部６４は、ステップ１１－１において回数ｉが０ではないすなわち初回でなれば（ステップ１１－１の「Ｎｏ」）、メイン制御部６４はピークホールド値τｄＰをそのままにする。
　ピークホールドブロック７４は、関節軸６７１Ａの角速度ωｍと他の関節軸６７２Ａ～６７６Ａの角速度ωｍのうちの最大の絶対値を有する角速度ωｍＭＡＸの絶対値が角速度閾値ωｍｔｈより大きいか否かを判定する（ステップ１１－２）。角速度閾値ωｍｔｈはピークホールドを実行するか否かを判定する閾値である。ピークホールドブロック７４は、ステップ１１－２において角速度ωｍＭＡＸの絶対値が角速度閾値ωｍｔｈより大きい場合は（ステップ１１－２の「Ｙｅｓ」）、ピークホールドの回数ｉを１だけ増加させる（ステップ１１－２Ａ）。ピークホールドブロック７４は、ステップ１１－３において角速度ωｍＭＡＸの絶対値が角速度閾値ωｍｔｈより大きくない場合は（ステップ１１－２の「Ｎｏ」）、処理を終了させる（ステップ１１－４Ｂ）。
　ステップ１１－２の条件を設ける理由は、ロボット１００１がユーザによる停止操作やセンサ信号入力待ち等で停止している場合には、粘性摩擦係数Ｄが高いことに起因する衝突トルク推定値τｄｉｓｏの誤差が発生しないからである。
　また、図１１に示す動作では、モータ６６の角速度ωｍを監視するが、ロボット１００１が動作していることを確認できる他のパラメータを角速度ωｍの代わりに監視してもよい。例えばロボット１００１のプログラム内で動作命令が実行されている時、位置指令が生成されている時等でロボット動作を判定しても良い。
　ピークホールドブロック７４は、トルク推定ブロック３０が動力学トルク推定値τｄｙｎｏを用いて求めた衝突トルク推定値τｄｉｓｏの絶対値がピークホールド値τｄＰより大きいか否かを判定する（ステップ１１－３）。ステップ１１－３にて衝突トルク推定値τｄｉｓｏの絶対値がピークホールド値τｄＰより大きければ（ステップ１１－３の「Ｙｅｓ」）、ピークホールドブロック７４は、ピークホールド値τｄＰを衝突トルク推定値τｄｉｓｏの絶対値にするように更新する。ステップ１１－３にて衝突トルク推定値τｄｉｓｏの絶対値がピークホールド値τｄＰより大きくなければ（ステップ１１－３の「Ｎｏ」）、ピークホールドブロック７４はピークホールド値τｄＰをそのままにする。
　ピークホールドブロック７４は、ピークホールドの回数ｉが終了回数ｎｕｍに到達して終了回数ｎｕｍ以上であるか否かを判定する（ステップ１１－４）。ステップ１１－４においてピークホールドの回数ｉが終了回数ｎｕｍ以上であれは（ステップ１１－４の「Ｙｅｓ」）、ピークホールドブロック７４は、ピークホールド値τｄＰを出力し、回数ｉをクリアして０にして（ステップ１１－４Ａ）、ステップ１１－４Ｂにおいて処理を終了する。ステップ１１－４においてピークホールドの回数ｉが終了回数ｎｕｍ以上でなければ（ステップ１１－４の「Ｎｏ」）、ピークホールドブロック７４は、回数ｉをそのままにしてステップ１１－４Ｂにおいて処理を終える。
　次に、ピークホールドブロック７４から出力された単位時間でのピークホールド値τｄＰをデータ蓄積ブロック７５で蓄積する。衝突トルク推定値τｄｉｓｏとしてのｘ回目の単位時間（例えば、単位時間が１分間であればｘ分目）のピークホールド値τｄＰをピークホールド値τｄＰ［ｘ］と定義する。現在がｘ回目の単位時間でＮ個のピークホールド値τｄＰを蓄積している場合には、データ蓄積ブロック７５はピークホールド値τｄＰ［ｘ－Ｎ＋１］、τｄＰ［ｘ－Ｎ＋２］、…、τｄＰ［ｘ］を蓄積する。
　例えば、衝突検出閾値τｔｈの切換の時点としての現在が３０回目の単位時間であり（ｘ＝３０）、切換の時点より所定時間前（ｘ回だけ前の単位時間で、例えば単位時間が１分であればｘ分前）としてのデータ蓄積個数Ｎが１０個（Ｎ＝１０）である場合には、データ蓄積ブロック７５はピークホールド値τｄＰ［３０］、τｄＰ［２９］、…、τｄＰ［２１］を蓄積していることになる。単位時間が１分間であると、ロボット１００１が動作している１０分間のデータを蓄積することになる。
　図１０の最大値抽出ブロック７６は、ピークホールド値τｄＰ［ｘ－Ｎ＋１］、τｄＰ［ｘ－Ｎ＋２］、…、τｄＰ［ｘ］のうちの最大値τｄＰＰを得る。
　図１２はエンコーダ５２に内蔵された温度センサ５６の温度センサ出力値Ｔｃと、ｘ回目のピークホールド値τｄＰ［ｘ］を示す。図１２において、縦軸は温度センサ出力値Ｔｃと衝突トルクであるピークホールド値τｄＰ［ｘ］とを示し、横軸は単位時間の回数ｘを示す。図１２において、縦軸の衝突トルクや閾値は、最大トルクに対する比である最大トルク比（％）で示している。図１２に示す０≦ｘ≦３０の時点ｘでは、実際に衝突は発生しておらず、温度の上昇と共に衝突トルク推定値τｄＰ［ｘ］が減少している場合、つまり粘性摩擦係数Ｄの減少による衝突トルク推定値τｄＰ［ｘ］の誤差が減少している場合を示している。
　図１２において、Ｔｃ≧Ｔｃｔｈの条件がｘ＝３０の時点で成立しており、このときの最大値τｄＰＰを示している。データ蓄積個数Ｎは１０個である。
　図１０の傾き演算ブロック７７は、衝突トルク推定値τｄＰ［ｘ－Ｎ＋１］～τｄＰ［ｘ］までの近似直線８３の傾きτｄＰＡを演算する。
　図１２において、近似直線８３はｘ＝３０の時点より所定時間前までの衝突トルクの推定値としてのピークホールド値τｄＰ［ｘ］を近似している。近似直線８３の傾きを演算して傾きτｄＰＡを得る。
　図１３は図１０の切換え許可判定ブロック７８が実行する処理を示すフローチャートである。次に切換え許可判定ブロック７８は図１３のフローチャートに示す処理を実行する。
　切換え許可判定ブロック７８は、まず、切換え許可値ＣＥを「１」以外の数例えば「０」に設定する（クリアする）（ステップ１３－０）。
　その後、切換え許可判定ブロック７８は、衝突検出閾値の切換の時点より所定時間前から衝突検出閾値の切換の時点までの期間Ｄｕでの衝突トルクの最大値である最大値τｄＰＰが最大値判定閾値τｄＰＰｔｈＡより小さいか否かを判定する（ステップ１３－１）。ステップ１３－１において最大値τｄＰＰが最大値判定閾値τｄＰＰｔｈＡより小さければ（ステップ１３－１の「Ｙｅｓ」）、切換え許可判定ブロック７８は切換え許可値ＣＥを「１」に設定して（ステップ１３－１Ａ）処理を終える。
　ステップ１３－１において最大値τｄＰＰが最大値判定閾値τｄＰＰｔｈＡより小さくない場合（ステップ１３－１の「Ｎｏ」）、切換え許可判定ブロック７８は、最大値τｄＰＰが最大値判定閾値τｄＰＰｔｈＢより小さいか否かを判定する（ステップ１３－２）。ステップ１３－２にて最大値τｄＰＰが最大値判定閾値τｄＰＰｔｈＢより小さくない場合には（ステップ１３－２の「Ｎｏ」）、処理を終える。ただし、最大値判定閾値τｄＰＰｔｈＡと最大値判定閾値τｄＰＰｔｈＢと常温時の衝突検出閾値τｔｈとはτｄＰＰｔｈＡ＜τｄＰＰｔｈＢ＜τｔｈの関係を満たすように設定する。
　ステップ１３－２にて最大値τｄＰＰが最大値判定閾値τｄＰＰｔｈＢより小さい場合には（ステップ１３－２の「Ｙｅｓ」）、切換え許可判定ブロック７８は、近似直線８３の傾きτｄＰＡが傾き判定閾値τｄＰＡｔｈより小さいか否かを判定する（ステップ１３－３）。ステップ１３－３において傾き判定閾値τｄＰＡｔｈより小さければ（ステップ１３－３の「Ｙｅｓ」）、切換え許可判定ブロック７８は切換え許可値ＣＥを「１」に設定して処理を終える。ステップ１３－３において傾き判定閾値τｄＰＡｔｈより小さくない場合には（ステップ１３－３の「Ｎｏ」）、切換え許可判定ブロック７８は何もせず処理を終える。
　図１２の例では、最大値判定閾値τｄＰＰｔｈＡ、τｄＰＰｔｈＢと傾き判定閾値τｄＰＡｔｈは、τｄＰＰｔｈＡ＝２０、τｄＰＰｔｈＢ＝３５、τｄＰＡｔｈ＝０と設定されている。ｘ＝３０の時点で、τｄＰＰ＜τｄＰＰｔｈＡが成立しているので、切換え許可判定ブロック７８はステップ１３－１で切換え許可値ＣＥを「１」に設定する。
　図９において、閾値切換ブロック４１は常温に対応する衝突検出閾値τｔｈと低温に対応する閾値増分ｄτｔｈとを用いて（数７）に従って衝突検出閾値τｖｔｈを衝突判定ブロックに出力する。
Figure JPOXMLDOC01-appb-M000007
　（数７）に示すように、閾値切換ブロック４１は、Ｔｃ≧ＴｃｔｈかつＣＥ＝１である場合には、衝突検出閾値τｖｔｈとして常温に対応する衝突検出閾値τｔｈを出力する。また、閾値切換ブロック４１は、Ｔｃ＜Ｔｃｔｈである場合には、許可値ＣＥの値に関わらず、衝突検出閾値τｖｔｈとして常温に対応する衝突検出閾値τｔｈと閾値増分ｄτｔｈとの和を出力する。
　なお、温度センサ出力値Ｔｃはモータ６６のエンコーダ５１の内部温度であり減速機５３の温度ではないので、温度閾値Ｔｃｔｈは衝突検出閾値τｔｈと閾値増分ｄτｔｈを定めた際の減速機５３の温度（例えば約５℃）にする必要はなく、マージンを考えて例えば１０℃に設定してもよい。
　図１４は閾値切換ブロック４１が実行する（数７）の処理を示すフローチャートである。
　閾値切換ブロック４１は、温度センサ出力値Ｔｃが所定の温度閾値Ｔｃｔｈ以上であるか否かを判定する（ステップ１４－１）。ステップ１４－１において温度センサ出力値Ｔｃが所定の温度閾値Ｔｃｔｈ以上でない場合には（ステップ１４－１の「Ｎｏ」）、衝突検出閾値τｖｔｈとして衝突検出閾値τｔｈと閾値増分ｄτｔｈとを加算して得られる（τｔｈ＋ｄτｔｈ）を出力し、許可値ＣＥに「０」を設定して処理を終える。
　ステップ１４－１において温度センサ出力値Ｔｃが所定の温度閾値Ｔｃｔｈ以上である場合には（ステップ１４－１の「Ｙｅｓ」）、閾値切換ブロック４１は切換え許可値ＣＥが「１」であるか否かを判定する（ステップ１４－２）。ステップ１４－２において切換え許可値ＣＥが「１」である場合には（ステップ１４－２の「Ｙｅｓ」）、閾値切換ブロック４１は衝突検出閾値τｖｔｈとして常温に対応する衝突検出閾値τｔｈを設定し（ステップ１４－２）、許可値ＣＥはそのままにして処理を終える。
　図１２においては、ｘ＝３０の時点で、図１４に示すＴｃ≧Ｔｃｔｈ（ステップ１４－１の「Ｙｅｓ」）とＣＥ＝１（ステップ１４－２の「Ｙｅｓ」）の両方が初めて満たされる。したがって、閾値切換ブロック４１は、ｘ＝３０の時点で衝突検出閾値τｖｔｈは低温に対応する値（τｔｈ＋ｄτｔｈ）から常温に対応する値τｔｈに低下させる。図１５は図１２のｘ＝３０の時点以後の温度センサ出力値Ｔｃとピークホールド値τｄＰとを示す。図１５において、縦軸は温度センサ出力値Ｔｃと衝突トルクのｘ回目のピークホールド値τｄＰ［ｘ］を示し、横軸は単位時間の回数ｘを示す。図１５において、図１２と同様に、縦軸の衝突トルクや閾値は、最大トルクに対する比である最大トルク比（％）で示している。
　図１５に示すように、閾値切換ブロック４１は、衝突検出閾値τｖｔｈはｘ＝３０の時点で、低温に対応する値７０％（＝４０＋３０）から常温に対応する値４０％に低下させる。
　図１５に示すように、Ｔｃ≧Ｔｃｔｈの条件が成立するｘ＝３０の時点において、衝突トルク推定値τｄｉｓｏ（衝突トルク推定誤差）が温度Ｔｃの上昇と共に小さくなる。これにより、最大値τｄＰＰが最大値判定閾値τｄＰＰｔｈＡより小さく、衝突検出閾値τｖｔｈの常温に対応する値τｔｈに比べて十分に小さい。このように、図１５の閾値［％］では、衝突検出閾値τｖｔｈを小さくしても衝突の誤検出が発生しにくい。
　以上の様に、実施の形態においてロボット１００１の衝突検出方法では、減速機５３を介してモータ６６により駆動されるロボット１００１において、モータ６６が減速機５３に出力するトルクからロボット１００１の逆動力学演算で求めた動力学トルクを差し引くことにより、衝突による外力トルクを衝突トルク推定値τｄｉｓｏとして推定する。衝突トルク推定値τｄｉｓｏが所定の衝突検出閾値τｔｈより大きければ、ロボット１００１が外力を受けたと判断する。モータ６６のエンコーダ５２に内蔵された温度センサ５６の出力値が所定の温度閾値より高ければ、衝突トルク推定値τｄｉｓｏの衝突検出閾値τｔｈを小さい値に切換える。衝突検出閾値τｔｈの切換えの時点より所定時間前（単位時間でｘ回前、例えば単位時間が１分であればｘ分前）からの衝突トルク推定値τｄｉｓｏの最大値τｄＰＰが、衝突トルク推定値τｄｉｓｏの衝突検出閾値τｔｈの切換え後の値より小さな最大値判定閾値τｄＰＰｔｈＡ未満であれば、衝突検出閾値τｔｈの切換えを許可する。
　これにより、エンコーダ５２内蔵の温度センサ５６を用いて衝突検出閾値τｔｈを切換える場合でも、衝突誤検出の発生を減じることができる。
　次に図１５以外の動作を説明する。
　図１６は実施の形態における衝突検出方法の他の動作における温度センサ出力値Ｔｃとピークホールド値τｄＰとを示す。図１６において、縦軸は温度センサ出力値Ｔｃと衝突トルクのｘ回目のピークホールド値τｄＰ［ｘ］を示し、横軸は単位時間の回数ｘを示す。図１６において、図１５と同様に、縦軸の衝突トルクや閾値は、最大トルクに対する比である最大トルク比（％）で示している。図１６に示す動作では図１５に比べ、温度センサ出力値Ｔｃの上昇より減速機５３の温度の上昇が低く、粘性摩擦係数Ｄが十分に低下していない。
　図１６において、Ｔｃ≧Ｔｃｔｈの条件が成立するｘ＝３０の時点において、最大値τｄＰＰが最大値判定閾値τｄＰＰｔｈＡ以上、最大値判定閾値τｄＰＰｔｈＢ（７２）未満である。この場合には、図１３のステップ１３－１の「Ｎｏ」とステップ１３－２の「Ｙｅｓ」の条件が成立し、ステップ１３－３の処理が実行される。
　図１６に示す動作では図１５に比べ、減速機５３の温度の上昇が低く粘性摩擦係数Ｄが十分に低下していないため、衝突検出閾値τｔｈを小さくした場合、衝突誤検出の発生の可能性が高くなる。そこで図１３のステップ１３－３において図１６のｘ＝３０の時点の近似直線８３の傾きτｄＰＡが傾き判定閾値τｄＰＡｔｈより小さい場合には、切換え許可判定ブロック７８はステップ１３－３Ａにおいて切換え許可値ＣＥを「１」に設定する。ステップ１３－３において図１６のｘ＝３０の時点の近似直線８３の傾きτｄＰＡが傾き判定閾値τｄＰＡｔｈより小さくない場合には切換え許可値ＣＥをそのままにして処理を終了する。
　図１６では、傾き判定閾値τｄＰＡｔｈを０に設定している。図１６に示す近似直線８３の傾きτｄＰＡは傾き判定閾値τｄＰＡｔｈすなわち０より小さいので（図１３のステップ１３－３の「Ｙｅｓ」）、衝突トルクが実際には減少傾向にあり衝突誤検出の可能性が低いと判断し、切換え許可判定ブロック７８は切換え許可値ＣＥを「１」に設定し、衝突検出閾値τｖｔｈはｘ＝３０の時点で低温に対応する値７０％（＝４０＋３０）から常温に対応する値４０％に低下させる。
　図１７は実施の形態における衝突検出方法のさらに他の動作における温度センサ出力値Ｔｃとピークホールド値τｄＰとを示す。図１７において、縦軸は温度センサ出力値Ｔｃと衝突トルクのｘ回目のピークホールド値τｄＰ［ｘ］を示し、横軸は単位時間の回数ｘを示す。図１７において、図１５や図１６と同様に、縦軸の衝突トルクや閾値は、最大トルクに対する比である最大トルク比（％）で示している。図１７は、図１６と比較し、ｘ＝３０の時点の近似直線８３の傾きτｄＰＡが傾き判定閾値τｄＰＡｔｈより大きい場合（τｄＰＡ＞０）の動作を示している。
　図１７に示す動作では、モータ６６の発熱によりエンコーダ５２内の温度センサ５６の温度センサ出力値Ｔｃが上昇しているにも関わらず、外気温が低く減速機５３の粘性摩擦係数Ｄが増加する場合に相当する。
　図１６と図１７では、傾き判定閾値τｄＰＡｔｈは０に設定している。
　この場合、衝突トルク推定値τｄｉｓｏ（衝突トルク推定誤差）は増加する傾向、すなわちピークホールド値τｄＰ［ｘ］が増加する傾向にあるので、衝突誤検出を防ぐため、図１３のステップ１３－３で「Ｎｏ」の判定となり、切換え許可値ＣＥは「１」に設定されず、衝突検出閾値τｖｔｈは低温に対応する値（７０％）から低下させない。
　図１８は図１７に示す動作でのｘ＝４０の時点での温度センサ出力値Ｔｃと衝突トルクのピークホールド値τｄＰとを更に示す。ｘ＝３０の時点以後も衝突トルク推定値τｄｉｓｏ（衝突トルク推定誤差）は増加する傾向すなわちピークホールド値τｄＰ［ｘ］が増加する傾向にあり、ｘ＝３１、３２、‥、４０で図１０に示す波形分析ブロック５７での処理が行われても、切換え許可値ＣＥが「１」に設定されない。したがって、ｘ＝４０の時点でも衝突検出閾値τｖｔｈは低温に対応する値（７０％）から低下させない。
　図１９は実施の形態における衝突検出方法のさらに他の動作における温度センサ出力値Ｔｃとピークホールド値τｄＰとを示す。図１９において、縦軸は温度センサ出力値Ｔｃと衝突トルクのｘ回目のピークホールド値τｄＰ［ｘ］を示し、横軸は単位時間の回数ｘを示す。図１９において、図１５から図１８と同様に、縦軸の衝突トルクや閾値は、最大トルクに対する比である最大トルク比（％）で示している。図１９に示す動作では、図１６に比べ、温度センサ５６の温度センサ出力値Ｔｃより減速機５３の温度上昇がさらに低く、粘性摩擦係数Ｄが低下していない場合の動作を示す。
　図１９においてＴｃ≧Ｔｃｔｈの条件が成立するｘ＝３０の時点で、最大値τｄＰＰは最大値判定閾値τｄＰＰｔｈＢより大きい。すなわち、図１３のステップ１３－１での「Ｎｏ」となる条件と、ステップ１３－２での「Ｎｏ」となる条件が成立している。
　図２０は図１９に示す動作でのｘ＝３０の時点以後の温度センサ出力値Ｔｃとピークホールド値τｄＰとを更に示す。
　図２０において、衝突トルク推定値τｄｉｓｏ（衝突トルク推定誤差）は減少する傾向すなわちピークホールド値τｄＰ［ｘ］が減少する傾向にあり、ｘ＝３５の時点で、図１３のステップ１３－１での「Ｎｏ」の条件とステップ１３－２での「Ｙｅｓ」の条件とステップ１３－３での「Ｙｅｓ」の条件が成立する。したがって、閾値切換ブロック４１は、衝突検出閾値τｖｔｈを低温に対応する値７０％（＝４０＋３０）から常温に対応する値４０％に低下させる。
　この様に、実施の形態におけるロボット１００１の衝突検出方法では、最大値判定閾値τｄＰＰｔｈＡより大きく、衝突検出閾値τｖｔｈの切換え後の値より小さな最大値判定閾値τｄＰＰｔｈＢが設けられる。衝突トルク推定値τｄｉｓｏの衝突検出閾値τｖｔｈの切換えの時点より所定時間前からの衝突トルク推定値τｄｉｓｏの最大値τｄＰＰが、最大値判定閾値τｄＰＰｔｈＡ以上で、最大値判定閾値τｄＰＰｔｈＢ未満であり、かつ、衝突検出閾値τｖｔｈの切換えの時点より所定時間前（単位時間でｘ回前、例えば単位時間が１分であればｘ分前）からの衝突トルク推定値τｄｉｓｏの時間に対する推移（ｘ回目のピークホールド値τｄＰ［ｘ］）を近似した近似直線の傾きτｄＰＡが負であれば、衝突トルク推定値τｄｉｓｏの衝突検出閾値τｖｔｈの切換えを許可する。
　これにより、エンコーダ５２内蔵の温度センサ５６の温度センサ出力値Ｔｃより減速機５３の温度上昇が低く、粘性摩擦係数Ｄが十分に低下していない場合であっても、衝突の誤検出の発生を減じることができる。
　上述のように、ロボット１００１は、モータ６６と、モータ６６に接続された減速機５３と、モータ６６の回転を検出するエンコーダ５１と、エンコーダ５１に内蔵された温度センサ５６と、減速機５３を介してモータ６６により駆動される対象物（アーム６７）とを有する。モータ６６が減速機５３に出力するトルクから、ロボット１００１の逆動力学演算で求めた動力学トルクを差し引くことにより、衝突による外力トルクを衝突トルク推定値τｄｉｓｏ（τｄＰ）として推定する。衝突トルク推定値τｄｉｓｏ（τｄＰ）が所定の衝突検出閾値τｖｔｈより大きければ、ロボット１００１が外力を受けたと判断する。温度センサ５６で検出された温度（Ｔｃ）が所定の温度閾値Ｔｃｔｈ未満である場合に、所定の衝突検出閾値τｖｔｈを値（τｔｈ＋ｄτｔｈ）にする。検出された温度（Ｔｃ）が所定の温度閾値Ｔｃｔｈ以上である時点（ｘ＝３０）より所定時間だけ前の時点（ｘ＝２０）から時点（ｘ＝３０）までの期間Ｄｕでの衝突トルク推定値τｄｉｓｏ（τｄＰ）の最大値τｄＰＰが最大値判定閾値τｄＰＰｔｈＡより小さい場合に、時点（ｘ＝３０）において所定の衝突検出閾値τｖｔｈを値（τｔｈ＋ｄτｔｈ）より小さい値τｔｈにする。期間Ｄｕでの衝突トルク推定値τｄｉｓｏ（τｄＰ）の最大値τｄＰＰが最大値判定閾値τｄＰＰｔｈＡ以上である場合に、時点（ｘ＝３０）において所定の衝突検出閾値τｖｔｈを値（τｔｈ＋ｄτｔｈ）にする。
　最大値判定閾値τｄＰＰｔｈＡより大きくかつ値τｔｈより小さな最大値判定閾値τｄＰＰｔｈＢを設定してもよい。期間Ｄｕでの衝突トルク推定値τｄｉｓｏ（τｄＰ）の時間推移を近似して得られる近似直線８３の傾きτｄＰＡを得てもよい。この場合に、期間Ｄｕでの衝突トルク推定値τｄｉｓｏ（τｄＰ）の最大値τｄＰＰが最大値判定閾値τｄＰＰｔｈＡ以上でかつ最大値判定閾値τｄＰＰｔｈＢ未満であり、近似直線８３の傾きτｄＰＡが負である場合に、時点（ｘ＝３０）において所定の衝突検出閾値τｖｔｈを値τｔｈにする。期間Ｄｕでの衝突トルク推定値τｄｉｓｏ（τｄＰ）の最大値τｄＰＰが最大値判定閾値τｄＰＰｔｈＡ以上でかつ最大値判定閾値τｄＰＰｔｈＢ未満であり、近似直線８３の傾きτｄＰＡが０または正である場合に、時点（ｘ＝３０）において所定の衝突検出閾値τｖｔｈを値（τｔｈ＋ｄτｔｈ）にする。期間Ｄｕでの衝突トルク推定値τｄｉｓｏ（τｄＰ）の最大値τｄＰＰが最大値判定閾値τｄＰＰｔｈＢ以上である場合に、時点（ｘ＝３０）において所定の衝突検出閾値τｖｔｈを値（τｔｈ＋ｄτｔｈ）にする。
　期間Ｄｕでの衝突トルク推定値τｄｉｓｏ（τｄＰ）の最大値τｄＰＰが最大値判定閾値τｄＰＰｔｈＢ以上である場合に、近似直線８３の傾きτｄＰＡに関わらず、時点（ｘ＝３０）において所定の衝突検出閾値τｖｔｈを値（τｔｈ＋ｄτｔｈ）にしてもよい。
　以上のように、実施の形態におけるロボット１００１の衝突検出方法により、エンコーダ５２内蔵の温度センサ５６を用いて衝突検出閾値τｖｔｈを切換える場合でも、衝突の誤検出の発生を減じることができる。
　本発明のロボットの衝突検出方法は、温度センサによる衝突検出閾値切換えにおいて、コスト上昇や信頼性の低下を招く減速機への温度センサ取り付けをせず、モータエンコーダの内蔵された温度センサを用いた場合でも、衝突誤検出の発生を減じることができ、産業上有用である。
６　　位置制御ブロック
１０　　速度制御ブロック
１７　　伝達関数ブロック
１８　　モータ及び外力を示すブロック
２１　　回転方向判定ブロック
２６　　トルク演算ブロック
３０　　トルク推定ブロック
３１，４０　　衝突判定ブロック
３２　　微分要素
３４，４１　　閾値切換ブロック
５１　　エンコーダ
５２　　エンコーダ
５３　　減速機
５６　　温度センサ
５７　　波形分析ブロック
６１，１６１　　ロボット機構
６２，１６２　　ロボット制御装置
６３　　操作・教示部
６４　　メイン制御部
６５　　サーボ制御部
６６　　モータ
６７　　アーム
６８　　ベルト
７４　　ピークホールドブロック
７５　　データ蓄積ブロック
７６　　最大値抽出ブロック
７７　　傾き演算ブロック
７８　　切換え許可判定ブロック
８３　　近似直線
６７Ａ（６７１Ａ～６７６Ａ）　　関節軸（対象物）
１００１　　ロボット
ＣＥ　　切換え許可値
ｄτｔｈ　　衝突検出閾値増分
Ｉｍ　　モータ電流
ＫＩ／ｓ　　速度積分ゲイン
ＫＰ　　速度比例ゲイン
ＫＰＰ　　位置比例ゲイン
Ｋｔ　　モータトルク定数
Ｋμ　　動摩擦係数
ｓｇｎ　　モータ方向信号
Ｔｃ　　温度センサ出力値
Ｔｃｔｈ　　温度閾値
αｃｏｍ　　加速度成分
θｃｏｍ　　位置指令
θＬ　　負荷位置
θｍ　　位置フィードバック
τｄｉｓ　　衝突トルク
τｄｉｓｏ　　衝突トルク推定値
τｄＰ　　ピークホールド値（衝突トルク推定値）
τｄＰ［ｘ］　　ピークホールド値
τｄＰＡ　　傾き
τｄＰＡｔｈ　　傾き判定閾値
τｄＰＰ　　最大値
τｄＰＰｔｈＡ　　最大値判定閾値（第１の最大値判定閾値）
τｄＰＰｔｈＢ　　最大値判定閾値（第２の最大値判定閾値）
τｄｙｎ　　動力学トルク
τｄｙｎｏ　　動力学トルク推定値
τｍ　　モータ発生トルク
τｔｈ　　衝突検出閾値
τｖｔｈ　　衝突検出閾値
τμ　　動摩擦トルク
τμｏ　　動摩擦トルク推定値
ωｃｏｍ　　速度ループ指令
ωｍ　　モータ角速度

Claims (3)

  1. モータと、前記モータに接続された減速機と、前記モータの回転を検出するエンコーダと、前記エンコーダに内蔵された温度センサと、前記減速機を介して前記モータにより駆動される対象物とを有するロボットを準備するステップと、
    前記モータが前記減速機に出力するトルクから、前記ロボットの逆動力学演算で求めた動力学トルクを差し引くことにより、衝突による外力トルクを衝突トルク推定値として推定するステップと、
    前記衝突トルク推定値が所定の衝突検出閾値より大きければ、前記ロボットが外力を受けたと判断するステップと、
    前記温度センサで検出された温度が所定の温度閾値未満である場合に、前記所定の衝突検出閾値を第１の値にするステップと、
    前記検出された温度が前記所定の温度閾値以上である第１の時点より所定時間だけ前の第２の時点から前記第１の時点までの期間での前記衝突トルク推定値の最大値が第１の最大値判定閾値より小さい場合に、前記第１の時点において前記所定の衝突検出閾値を前記第１の値より小さい第２の値にするステップと、
    前記期間での前記衝突トルク推定値の前記最大値が前記第１の最大値判定閾値以上である場合に、前記第１の時点において前記所定の衝突検出閾値を前記第１の値にするステップと、
    を含む、ロボットの衝突検出方法。
  2. 前記第１の最大値判定閾値より大きくかつ前記第２の値より小さな第２の最大値判定閾値を準備するステップと、
    前記期間での前記衝突トルク推定値の時間推移を近似して得られる近似直線の傾きを得るステップと、
    をさらに備え、
    前記期間での前記衝突トルク推定値の前記最大値が前記第１の最大値判定閾値以上である場合に、前記第１の時点において前記所定の衝突検出閾値を前記第１の値にする前記ステップは、
    　　　前記期間での前記衝突トルク推定値の前記最大値が前記第１の最大値判定閾値以上でかつ前記第２の最大値判定閾値未満であり、前記近似直線の前記傾きが負である場合に、前記第１の時点において前記所定の衝突検出閾値を前記第２の値にするステップと、
    　　　前記期間での前記衝突トルク推定値の前記最大値が前記第１の最大値判定閾値以上でかつ前記第２の最大値判定閾値未満であり、前記近似直線の前記傾きが０または正である場合に、前記第１の時点において前記所定の衝突検出閾値を前記第１の値にするステップと、
    　　　前記期間での前記衝突トルク推定値の前記最大値が前記第２の最大値判定閾値以上である場合に、前記第１の時点において前記所定の衝突検出閾値を前記第１の値にするステップと、
    を含む、請求項１に記載のロボットの衝突検出方法。
  3. 前記期間での前記衝突トルク推定値の前記最大値が前記第２の最大値判定閾値以上である場合に、前記第１の時点において前記所定の衝突検出閾値を前記第１の値にする前記ステップは、前記期間での前記衝突トルク推定値の前記最大値が前記第２の最大値判定閾値以上である場合に、前記近似直線の前記傾きに関わらず、前記第１の時点において前記所定の衝突検出閾値を前記第１の値にするステップを含む、請求項２に記載のロボットの衝突検出方法。
PCT/JP2016/003855 2015-09-16 2016-08-24 ロボットの衝突検出方法 Ceased WO2017047009A1 (ja)

Priority Applications (4)

Application Number Priority Date Filing Date Title
JP2017540469A JP6706741B2 (ja) 2015-09-16 2016-08-24 ロボットの衝突検出方法
EP16845887.5A EP3351356B1 (en) 2015-09-16 2016-08-24 Robot collision detection method
US15/580,832 US10690558B2 (en) 2015-09-16 2016-08-24 Robot collision detection method
CN201680041206.6A CN107848126B (zh) 2015-09-16 2016-08-24 机器人的碰撞检测方法

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
JP2015-182637 2015-09-16
JP2015182637 2015-09-16

Publications (1)

Publication Number Publication Date
WO2017047009A1 true WO2017047009A1 (ja) 2017-03-23

Family

ID=58288494

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/JP2016/003855 Ceased WO2017047009A1 (ja) 2015-09-16 2016-08-24 ロボットの衝突検出方法

Country Status (5)

Country Link
US (1) US10690558B2 (ja)
EP (1) EP3351356B1 (ja)
JP (1) JP6706741B2 (ja)
CN (1) CN107848126B (ja)
WO (1) WO2017047009A1 (ja)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2018027586A (ja) * 2016-08-17 2018-02-22 Dmg森精機株式会社 工具交換装置
CN108748158A (zh) * 2018-06-12 2018-11-06 上海节卡机器人科技有限公司 协作机器人、碰撞检测系统和方法、存储介质、操作系统
WO2020027118A1 (ja) * 2018-08-02 2020-02-06 株式会社神戸製鋼所 ロボット制御装置、ロボット制御方法及びロボット制御プログラム
WO2024224966A1 (ja) * 2023-04-27 2024-10-31 パナソニックＩｐマネジメント株式会社 ロボット制御方法

Families Citing this family (18)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP6805811B2 (ja) * 2016-12-26 2020-12-23 株式会社デンソーウェーブ マニプレータの動作判定装置
ES2965635T3 (es) * 2018-02-21 2024-04-16 Bae Systems Plc Robot con amortiguación activa
GB2575113B (en) * 2018-06-29 2022-06-08 Cmr Surgical Ltd Detecting collisions of robot arms
CN109093623B (zh) * 2018-08-24 2020-11-03 北京秘塔网络科技有限公司 一种基于闭环结构的扭矩计算、碰撞检测与拖动示教方法
KR102149008B1 (ko) 2018-12-13 2020-08-31 (주)미래컴퍼니 수술용 로봇의 충돌을 완화시키는 방법 및 시스템
DE102018133472B3 (de) * 2018-12-21 2020-03-12 Franka Emika Gmbh Bewegungsüberwachung eines Robotermanipulators
CN111347416B (zh) * 2018-12-24 2021-10-22 核动力运行研究所 一种无外部传感器的检查机器人碰撞检测方法
KR20200094396A (ko) 2019-01-30 2020-08-07 삼성전자주식회사 전자 장치 및 복수의 액션들을 포함하는 태스크를 결정하는 방법
JP6570782B1 (ja) * 2019-04-19 2019-09-04 あいおいニッセイ同和損害保険株式会社 衝突判定方法、衝突判定システム及びコンピュータプログラム
CN110587665B (zh) * 2019-09-02 2022-04-08 埃夫特智能装备股份有限公司 一种工业机器人关节碰撞保护方法
CN114270197B (zh) * 2019-09-27 2024-11-29 松下知识产权经营株式会社 冲击检测装置以及蓄电组
EP4052864B1 (en) * 2019-10-30 2025-08-27 Neuromeka Method for automatically setting collision sensitivity of collaborative robot
CN111267105A (zh) * 2020-03-18 2020-06-12 无锡砺成智能装备有限公司 一种六关节机器人的动力学参数辨识及碰撞检测方法
DE112020007659T5 (de) * 2020-10-05 2023-08-03 Mitsubishi Electric Corporation Roboterkollisionsdetektionsvorrichtung
CN112549024B (zh) * 2020-11-26 2022-05-24 华南理工大学 基于时间序列分析的机器人免传感碰撞检测方法及应用
CN115183924B (zh) * 2021-04-01 2024-11-08 广东博智林机器人有限公司 碰撞力值检测装置及其检测方法
CN115389077B (zh) * 2022-08-26 2024-04-12 法奥意威(苏州)机器人系统有限公司 碰撞检测方法、装置、控制设备及可读存储介质
CN115922779B (zh) * 2022-11-17 2025-01-28 上海飒智智能科技有限公司 机械臂外力碰撞检测处理方法及装置、嵌入式伺服驱动器

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH1115511A (ja) * 1997-06-20 1999-01-22 Fanuc Ltd ロボットの衝突検知方式
JP2005279872A (ja) * 2004-03-30 2005-10-13 Matsushita Electric Ind Co Ltd ロボットの制御方法
JP2009184095A (ja) * 2008-02-08 2009-08-20 Fanuc Ltd 部品保護機能を備えたロボット制御装置及びロボット制御方法

Family Cites Families (17)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2002144277A (ja) 2000-11-08 2002-05-21 Denso Corp ロボット
JP4756618B2 (ja) 2001-03-21 2011-08-24 株式会社ダイヘン 多関節ロボットにおける衝突検出・停止制御法
US6678582B2 (en) * 2002-05-30 2004-01-13 Kuka Roboter Gmbh Method and control device for avoiding collisions between cooperating robots
DE10327600A1 (de) * 2003-06-18 2005-01-27 Siemens Ag Vorrichtung und Verfahren zur Kollisionsüberwachung einer Maschinenkomponente mit einem Werkstück oder anderen Maschinenkomponenten
EP1652634B1 (en) * 2003-07-29 2011-12-28 Panasonic Corporation Robot arm control method and control device
JP3933158B2 (ja) * 2004-10-21 2007-06-20 松下電器産業株式会社 ロボットの衝突検出方法
JP2006167820A (ja) 2004-12-13 2006-06-29 Toyota Motor Corp ロボットアームの制御方法
JP2006281421A (ja) * 2005-04-05 2006-10-19 Yaskawa Electric Corp ロボットおよびロボットの異常検出方法
JP2007286904A (ja) 2006-04-17 2007-11-01 Fanuc Ltd 電動機の制御装置及び制御方法
JP5183751B2 (ja) * 2008-12-26 2013-04-17 三菱電機株式会社 衝突検知装置
EP2572838A1 (en) * 2010-08-31 2013-03-27 Kabushiki Kaisha Yaskawa Denki Robot, robot system, robot control device, and state determining method
CN102554939B (zh) * 2010-12-30 2014-12-10 沈阳新松机器人自动化股份有限公司 工业机器人碰撞保护方法和装置
JP5149416B2 (ja) * 2011-04-06 2013-02-20 ファナック株式会社 ロボットの異常検出機能を有するロボットシステム及びその制御方法
CN103192413A (zh) * 2012-01-06 2013-07-10 沈阳新松机器人自动化股份有限公司 一种无传感器的机器人碰撞检测保护装置及方法
JP2013169609A (ja) * 2012-02-20 2013-09-02 Panasonic Corp ロボットの衝突検出方法
JP5912683B2 (ja) 2012-03-07 2016-04-27 株式会社神戸製鋼所 モータ駆動構造を備えたシステム、モータ駆動構造のシステムに用いられるプログラム、および溶接物製造方法
JP5927440B2 (ja) * 2012-10-25 2016-06-01 パナソニックＩｐマネジメント株式会社 ロボットの異常表示方法

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH1115511A (ja) * 1997-06-20 1999-01-22 Fanuc Ltd ロボットの衝突検知方式
JP2005279872A (ja) * 2004-03-30 2005-10-13 Matsushita Electric Ind Co Ltd ロボットの制御方法
JP2009184095A (ja) * 2008-02-08 2009-08-20 Fanuc Ltd 部品保護機能を備えたロボット制御装置及びロボット制御方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
See also references of EP3351356A4 *

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2018027586A (ja) * 2016-08-17 2018-02-22 Dmg森精機株式会社 工具交換装置
CN108748158A (zh) * 2018-06-12 2018-11-06 上海节卡机器人科技有限公司 协作机器人、碰撞检测系统和方法、存储介质、操作系统
WO2020027118A1 (ja) * 2018-08-02 2020-02-06 株式会社神戸製鋼所 ロボット制御装置、ロボット制御方法及びロボット制御プログラム
JP2020019117A (ja) * 2018-08-02 2020-02-06 株式会社神戸製鋼所 ロボット制御装置、ロボット制御方法及びプログラム
JP7105646B2 (ja) 2018-08-02 2022-07-25 株式会社神戸製鋼所 ロボット制御装置、ロボット制御方法及びプログラム
WO2024224966A1 (ja) * 2023-04-27 2024-10-31 パナソニックＩｐマネジメント株式会社 ロボット制御方法

Also Published As

Publication number Publication date
EP3351356A4 (en) 2018-10-24
JPWO2017047009A1 (ja) 2018-06-28
EP3351356A1 (en) 2018-07-25
JP6706741B2 (ja) 2020-06-10
CN107848126B (zh) 2021-03-23
US20180164170A1 (en) 2018-06-14
US10690558B2 (en) 2020-06-23
CN107848126A (zh) 2018-03-27
EP3351356B1 (en) 2020-11-18

Similar Documents

Publication Publication Date Title
JP6706741B2 (ja) ロボットの衝突検出方法
US8442685B2 (en) Robot control apparatus
CN104718055B (zh) 机器人的异常显示方法
JP5902425B2 (ja) ロボット制御装置、外乱判定方法およびアクチュエータ制御方法
US10300600B2 (en) Control system having learning control function and control method
CN104507645B (zh) 机器人装置及其控制方法
JP5024383B2 (ja) ロボットの異常判定方法
JP3933158B2 (ja) ロボットの衝突検出方法
JP6975886B2 (ja) ロボットの制御方法
JPWO2005009692A1 (ja) ロボットアームの制御方法および制御装置
US9952249B2 (en) Inertia estimating method and inertia estimation apparatus of position control apparatus
WO2017073052A1 (ja) ロボットシステムの監視装置
WO2003068464A1 (en) Drive control method and drive controller
CN111347421A (zh) 用于扭矩估计的方法和装置
JP2013169609A (ja) ロボットの衝突検出方法
CN108372505A (zh) 一种基于动力学的机器人的碰撞检测算法及系统
US20170261529A1 (en) Method for identifying friction parameter for linear module
JP2005100143A (ja) モータ駆動装置の制御方法及び制御装置
CN113021353B (zh) 机器人碰撞检测方法
JPH11282540A (ja) ロボットの制御装置とその制御方法
JP2001353687A (ja) ロボットの制御装置とその制御方法
JP2011015550A (ja) 機械装置の制御装置及び機械装置の特性同定方法
Xu et al. Robot collision detection based on dynamic model
WO2024224966A1 (ja) ロボット制御方法
JP5849455B2 (ja) ロボット

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 16845887

Country of ref document: EP

Kind code of ref document: A1

ENP Entry into the national phase

Ref document number: 2017540469

Country of ref document: JP

Kind code of ref document: A

WWE Wipo information: entry into national phase

Ref document number: 15580832

Country of ref document: US

NENP Non-entry into the national phase

Ref country code: DE

WWE Wipo information: entry into national phase

Ref document number: 2016845887

Country of ref document: EP