Classifications

• • 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
  • G05B19/00—Programme-control systems
  • G05B19/02—Programme-control systems electric
  • G05B19/18—Numerical control [NC], i.e. automatically operating machines, in particular machine tools, e.g. in a manufacturing environment, so as to execute positioning, movement or co-ordinated operations by means of programme data in numerical form
  • G05B19/406—Numerical control [NC], i.e. automatically operating machines, in particular machine tools, e.g. in a manufacturing environment, so as to execute positioning, movement or co-ordinated operations by means of programme data in numerical form characterised by monitoring or safety
  • G05B19/4061—Avoiding collision or forbidden zones
• • 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/1628—Programme controls characterised by the control loop
  • B25J9/1651—Programme controls characterised by the control loop acceleration, rate control
• • 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
  • B25J9/1676—Avoiding collision or forbidden zones
• • 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/37—Measurements
  • G05B2219/37624—Detect collision, blocking by measuring change of velocity or torque
• • 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/49—Nc machine tool, till multiple
  • G05B2219/49162—On collision, obstruction reverse drive, accelerate, cancel inertia

Definitions

• the present invention relates to a drive control method and a drive control device, and more particularly to a method for detecting a collision of a driven member.
• Japanese Patent Application Laid-Open No. H5-2083994 proposes a method for detecting a collision based on disturbance of a signal of a torque sensor attached to a motor for driving a lopot arm.
• Japanese Patent Application Publication No. 666893 and Japanese Patent Application Laid-Open No. Hei 11-110490 disturbance torque received by a servomotor driving a lopot arm by an observer is estimated, and an obstacle is determined based on the estimated disturbance torque.
• a method for detecting a collision with a vehicle has been proposed.
• Japanese Patent Application Laid-Open No. Hei 8-222964 discloses a method of controlling a robot based on a method of detecting a collision based on a deviation between a position of a movable part and an actual position instructed by a robot controller.
• a method has been proposed in which a theoretical position deviation is calculated based on the delay time of the system, and a collision is detected based on a comparison result between the theoretical position deviation and an actual position deviation.
• Japanese Patent Application Laid-Open No. 7-143780 discloses that when a collision is detected, torque is applied to the motor in a direction opposite to the driving direction. In addition, we propose a method to reduce the time from collision detection to the stop of the robot collision.
• the applicant of the present application calculates the theoretical torque from the Lopot's equation of motion, and then calculates the theoretical current value of the theoretical torque-driven servo motor.
• a drive control method and a drive control device that determine that a collision has occurred have already been proposed (Japanese Patent Application Laid-Open No. 2000-111176). ).
• the present invention has been made in view of the problems of the related art, and provides a drive control method and a drive control device capable of accurately detecting a collision of a driven member driven by a drive device in a robot or the like with a simple configuration. Its primary purpose is to provide.
• a second object of the present invention is to provide a drive control method and a drive control device that can minimize damage to members caused by a collision.
• a drive control method is provided.
• the drive control device controls the driven member to move via the drive device, and in the drive control method or the drive control device that detects the collision of the driven member, the drive control device calculates the estimated deviation from the actual speed of the driven member.
• a collision of the driven member is detected based on a certain estimated speed deviation or an estimated acceleration deviation that is an estimated deviation from the actual acceleration of the driven member.
• collision means a collision between the driven member and another member.
• the collision is detected based on the estimated speed deviation or the estimated acceleration deviation, so that the detection accuracy can be improved while simplifying the configuration of the drive control device.
• the collision processing means is provided, the time required for mounting the control target device can be reduced. Furthermore, since it is not necessary to solve the equation of motion, the time required for collision detection can be reduced.
• the estimated speed deviation or the estimated acceleration deviation may be obtained based on a position command for moving the driven member and a detected value of the position of the driven member.
• both the estimated speed deviation and the estimated acceleration deviation are obtained, and the collision is determined by determining that the collision has occurred when any of the estimated speed deviation and the estimated acceleration deviation exceeds a respective threshold. May be detected. With this configuration, the collision can be detected more accurately.
• the position command value is subjected to fill processing having a time constant equivalent to the time constant of the driven member in the control via the driving device, thereby estimating the driven member.
• the estimated speed deviation or the estimated acceleration deviation may be obtained based on the obtained estimated position and the detected value of the position of the driven member.
• a collision process for moving the driven member in a direction opposite to that before the collision may be further performed based on the detection of the collision.
• damage to the member caused by the collision can be minimized.
• the device to be controlled can be quickly restarted.
• a second position command may be generated and used in the control in place of the first position command.
• the movement of the driven member is further controlled to maintain the current operation.
• the maintenance of the current operation of the driven member is stopped by the control. Thereafter, the driven member may be moved in the opposite direction as before the collision. With this configuration, in the case where the driven member involves a pressing operation, the driven member can be quickly moved backward after the collision.
• the device to be controlled is a robot drive control device or a drive that is a robot having the driven member as an end effector and the drive device. It consists of a control method. With such a configuration, the present invention can be applied to robot control.
• the arm of the lopot includes a plurality of the driving devices, an end effector and a link, and the driving device and the link are alternately connected from a base end to a tip end, and are closest to the tip end.
• the end effector is configured to be connected to the driving device connected to the link that is located, and a portion of the arm that is located on the distal end side from each driving device forms the driven member of each driving device. May be.
• FIG. 1 is a schematic diagram showing a configuration of hardware of a robot which is a device to be controlled by a drive control device according to an embodiment of the present invention.
• FIG. 2 is a diagram showing the control of the drive control device according to the present embodiment with the lopot of FIG. It is a block diagram which shows the structure of a control system.
• FIG. 3 is a block diagram showing a detailed configuration of the drive control device in FIG. 2 [Best Mode for Carrying Out the Invention]
• FIG. 1 is a schematic diagram showing a hardware configuration of a mouth port which is a device to be controlled by a drive control device according to an embodiment of the present invention.
• lopots are schematically shown using graphic symbols representing motor functions.
• the robot 1 has first, second, and third axes 2, 3, and 4 that are rotation axes, and fourth, fifth, and sixth axes 5, 6, that are rotation axes.
• the arm 8 has a 6-8 degree of freedom, and an end effector 9 as a driven member such as a hand or a welding tool is attached to a tip portion of the arm 8.
• Each of the axes 2, 3, 4, 5, 6, 7 of the arm 8 is driven by a servo mechanism including a support motor (not shown).
• the arm 8 has a plurality of (here, 7) links 10 1 to: L 07 are connected by the rotation axes 2, 3, 4 or the rotation axes 5, 6, 7 to be at the forefront.
• the end effector 9 is configured to be connected to the located link 107. Then, the rotating shafts 2, 3, and 4 relatively rotate the two links connected to each other around their axes. Further, the pivot shafts 5, 6, and 7 relatively rotate two links connected to each other around an axis orthogonal to their axis. This makes it possible for the end effector 9 of the arm 8 to move in a three-dimensional direction within a predetermined range and to change its posture.
• "a member moves” means "it moves or its posture changes”.
• the symbol G in FIG. 1 indicates an obstacle.
• FIG. 2 is a block diagram showing a configuration of a control system between the lopot of FIG. 1 and the drive control device according to the present embodiment.
• the drive control device 10 includes collision processing means 20.
• the drive control device 10 controls the movement of the arm 8 via the servo mechanism of the robot 1.
• This drive control device 10 has the same configuration as a normal robot controller except that it has collision processing means 20. Have been.
• the main body of the drive control device 10 is configured by a computer.
• the support mechanism is constituted by a servomotor provided on each of the rotating shafts 2, 3, 4 or the rotating shafts 5, 6, 7 in FIG.
• FIG. 3 is a block diagram showing a detailed configuration of the drive control device shown in FIG. 2.
• a drive control device 10 'constitutes a service mechanism of the mouth pot 1 shown in FIG.
• a plurality of (6 in the present embodiment) ServoMount M is provided for each.
• the drive control device 10 shown in FIG. 2 includes the drive control devices 10 ′ shown in FIG.
• an arithmetic unit for performing various operations (subtraction, differentiation, integration, etc.) shown in FIG. 3 is realized by software stored in a computer constituting the main body of the drive control device 10.
• these arithmetic units may be realized by hardware such as an electric circuit.
• the drive control device 10 includes an encoder 28 (position detector), a third differentiator 29, a second calculator (position deviation calculator) 32, a first proportional unit 33, Third computing unit (speed deviation calculator) 34, second proportional unit 35, third proportional unit 36, integrator 37, fourth computing unit 38, collision processing Means 20.
• the collision processing means 20 includes a switching switch 31 as described later.
• the changeover switch 31 has an external position command value (the value of the first position command) from the drive control device 10 shown in FIG. 2 and an internal position command value (the first position command value) from the internal position command value generator 30 described later. 2), and the switching switch 31 switches and outputs the inputted external position command value and internal position command value based on a collision detection signal described later.
• encoder 28 is connected to the spindle of Servomotor M, and this encoder 28 Detects the rotation angle (detection position: hereinafter referred to as encoder value) of the servo motor M from the reference position (reference angle).
• This encoder value is the position of the driven member of the sub-portion M (the distal end of the arm 8 of the port 1 in FIG. 1 with respect to the rotation axis or the rotation axis corresponding to the sub-portion M). Part).
• This encoder value is differentiated by a third differentiator 29 to calculate an actual speed (actual speed: hereinafter, referred to as a feedback speed).
• the encoder value and an external position command value or an internal position command value (hereinafter, simply referred to as a position command value) output from the switching switch 31 are input to a second computing unit 32, and a second
• the calculator 32 subtracts the encoder value from the position command value to calculate a position deviation.
• the first proportional unit 33 multiplies this position deviation by a predetermined value and converts it to speed.
• the third calculator 34 subtracts the feedback speed output from the third differentiator 29 from the converted speed (hereinafter, referred to as a conversion speed) to calculate a speed deviation.
• the second proportional unit 35 multiplies this speed deviation by a predetermined value and converts it into a current command value (hereinafter referred to as a primary current command value).
• the primary current command value is multiplied by a predetermined value by the third proportional unit 36 to generate a corrected current command value.
• the integrator 37 integrates this corrected current command value.
• the fourth computing unit 38 adds the current value (hereinafter, referred to as an integral current command value) integrated by the integrator 37 and the primary current command value.
• the current command value added by the fourth calculator 38 is input to the thermometer M as a command current command value.
• the primary current command value and the integrated current command value are added to obtain the command current value in order to maintain the current operation of the end effector 9. That is, the integrator 37 functions as a current operation maintaining instruction value generator. By setting this command value to zero, the maintenance of the current operation is stopped.
• the first proportional unit 33 is the same as a proportional unit used in a normal robot controller that converts a position command value into a speed
• the second proportional unit 35 is a normal robot controller. It is the same as a proportional device that converts the conversion speed used in the controller to a current command value.
• the collision processing means 20 ′ has a time constant equivalent to that of the mouth port 1, and is provided with a filter (estimated position calculator) 21 which filters an external position command value input from the drive control device 10.
• the first differentiator (estimated speed calculator) 22 that calculates the speed (hereinafter referred to as the estimated speed) by differentiating the external position command value (hereinafter referred to as the estimated position) that has been processed
• the first arithmetic unit (estimated speed deviation calculator) 23 that calculates the estimated speed deviation by subtracting the feedback speed output from the third differentiator 29 from the speed, and the estimated speed deviation sets the threshold value
• a first determiner 24 for determining whether or not the difference has been exceeded
• a second differentiator (estimated acceleration deviation calculator) 25 for differentiating the estimated speed deviation to calculate an estimated acceleration deviation
• Second decision unit 2 that decides whether the estimated acceleration deviation exceeds the threshold 6, an OR circuit 27 to which signals from the first and second decision units 24 and 26 are inputted, and an encoder value which is recorded and, if necessary, a position command value from the recorded encoder value.
• an internal position command value to which signals from the first and second decision units 24 and 26 are inputted, and an encoder value which is
• the OR circuit 27 outputs an ON signal (collision detection signal) when the estimated speed deviation or the estimated acceleration deviation exceeds the threshold, and the output signal is output from the integrator 37 and the internal position command value generation. It is input to the section 30 and the switching switch 31.
• the reason for using the estimated acceleration deviation is to enable early detection of the collision because the effect of the collision appears in the acceleration change faster than the speed change.
• the integrator 37 clears its integrated value. That is, the command value of the current operation maintenance command value generator is set to zero. This allows
• the internal position command value generator 30 receives the collision detection signal from the R circuit 27. When input, it outputs a position command value that reverses the stored position. That is, a position command value for causing the arm 8 of the robot 1 to perform the backward movement is generated.
• the stored position data is the position data in the latest predetermined time range.
• the switching switch 31 switches the position command value from the external position command value to the internal position command value.
• drive control device 10 ′ configured as described above.
• an external position command value is output from drive control 10. Then, in a normal state, the collision detection signal is not output as described later, so that the switching switch 31 outputs the external position command value.
• This external position command value is transferred to the second computing unit 32, the first proportional unit 33, the third computing unit 34, the second proportional unit 35, the third proportional unit 36, the integrator 3 7, and the fourth computing unit 38 generate the indicated current value using the encoder value output from the encoder 28 and the feedback speed, which is a derivative thereof, and input this to the thermometer M .
• the feedback control of the servo motor M is performed based on the external command value.
• the movement of the arm 8 of the mouth port 1 is controlled by the drive control device 10 via the servo mechanism.
• the external position command value is input to the filter 21 and converted into an estimated position.
• the first differentiator 22 differentiates this estimated position to calculate an estimated speed. Using this estimated speed and the aforementioned feedback speed, the first computing unit 23 calculates an estimated speed deviation.
• a second differentiator 25 differentiates the estimated speed deviation to calculate an estimated acceleration deviation. Then, the first determiner 24 determines whether or not the calculated estimated speed deviation exceeds a threshold.
• arm 8 of robot 1 operates so as to follow the external command value. Therefore, the estimated speed deviation is small. Therefore, the first decision unit
• Step 24 determines that the estimated speed deviation does not exceed the threshold.
• the second determiner 26 determines whether or not the calculated estimated acceleration deviation exceeds a threshold value.Here, the arm 8 of the robot 1 operates so as to follow the external command value. Therefore, the estimated acceleration deviation is small. Therefore, the second determiner 26 determines that the estimated acceleration deviation does not exceed the threshold. Therefore, the OR circuit 27 does not output the collision detection signal.
• the operation at the time of a collision will be described.
• the end effector 9 of the mouth port 1 collides with the obstacle G.
• the arm 8 since the arm 8 does not follow the external command value, the estimated speed deviation calculated by the first computing unit 23 and the estimated acceleration deviation calculated by the second differentiator 25 become large. Therefore, first determiner 24 determines that the estimated speed deviation exceeds the threshold.
• the second determiner 26 determines that the estimated acceleration deviation exceeds the threshold.
• the OR circuit 27 outputs a collision detection signal.
• the collision detection signal is input to the integrator 37, and the output of the integrator 37 becomes zero, thereby reducing the pressing force on the obstacle G by the end effector 9 of the mouth port 1 Is done. Further, the collision detection signal is an internal position command value generation unit.
• the collision detection signal is input to the switch 31 and the switch 31 outputs the internal command value.
• the satellite M moves in the opposite direction to that before the collision.
• the end effector 9 of the mouth port 1 quickly retreats and quickly leaves the obstacle G.
• the collision occurs when the estimated speed deviation or the estimated acceleration deviation exceeds the threshold value, so that the detection is performed while simplifying the configuration of the drive control device 10.
• the accuracy can be improved, and the time required for mounting the collision processing means 20 on the robot can be shortened. Also, since it is not necessary to solve the equation of motion, the time required for collision detection can be reduced.
• the integrator 37 is cleared simultaneously with the collision detection Since the maintenance of the movement toward the obstacle G on the evening of the evening 9 is stopped, the robot 1 can quickly retreat after the collision. In addition, since the mouth robot 1 is caused to retreat after the collision, the robot 1 can be restarted quickly.
• the collision processing means 20 ′ is provided for each of the support motors M, The mouth port 1 operates in the same manner as described above even when a portion of the arm 8 of the robot 1 on the tip side of the link 102 collides with the obstacle G.
• the present invention has been described based on the above embodiments, but the present invention is not limited to only these embodiments, and various modifications are possible.
• the collision detection is performed using both the estimated speed deviation and the estimated acceleration deviation.
• the collision detection may be performed based on either the estimated speed deviation or the estimated acceleration deviation. In that case, the configuration of the drive control device is further simplified.
• the drive control device according to the present invention is useful as a robot controller.
• the drive control method according to the present invention is useful as a drive control method for a mouth pot.

Landscapes

• Engineering & Computer Science (AREA)
• Robotics (AREA)
• Mechanical Engineering (AREA)
• Human Computer Interaction (AREA)
• Manufacturing & Machinery (AREA)
• Physics & Mathematics (AREA)
• General Physics & Mathematics (AREA)
• Automation & Control Theory (AREA)
• Manipulator (AREA)
• Numerical Control (AREA)
• Control Of Position Or Direction (AREA)
• Steering Control In Accordance With Driving Conditions (AREA)
• Vending Machines For Individual Products (AREA)
• Vehicle Body Suspensions (AREA)
• Character Spaces And Line Spaces In Printers (AREA)
• Control Of Velocity Or Acceleration (AREA)

Priority Applications (4)

Applications Claiming Priority (2)

Publications (1)

Family

ID=27678269

Family Applications (1)

Country Status (7)

Cited By (3)

Families Citing this family (34)

Citations (11)

Family Cites Families (9)

• 2002
  • 2002-02-18 JP JP2002039761A patent/JP2003236787A/ja active Pending
• 2003
  • 2003-02-18 WO PCT/JP2003/001685 patent/WO2003068464A1/ja not_active Ceased
  • 2003-02-18 DE DE60325558T patent/DE60325558D1/de not_active Expired - Lifetime
  • 2003-02-18 AT AT03705257T patent/ATE419096T1/de not_active IP Right Cessation
  • 2003-02-18 US US10/504,770 patent/US7102311B2/en not_active Expired - Lifetime
  • 2003-02-18 AU AU2003211363A patent/AU2003211363A1/en not_active Abandoned
  • 2003-02-18 EP EP03705257A patent/EP1477284B1/en not_active Expired - Lifetime

Patent Citations (12)

Also Published As

Similar Documents

Legal Events