A method for detecting a collision of a robot arm of an injection molding machine

Abstract

The application discloses a kind of injection molding machine mechanical hand collision detection method, it utilizes acquisition module to collect the data of each axis servo motor and carries out filtering to data, utilizes calibration module according to filtered data when first mode of full-automatic operation determines the motion state of each axis and calibrates the maximum torque value and minimum torque value under this motion state, utilizes calibration module to calibrate the speed time delay between the feedback speed and command speed of each axis simultaneously, utilizes collision detection module according to the command speed and speed time delay of each axis to predict actual speed and actual acceleration when running each mode subsequently determines the motion state of each axis, and according to the motion state of each axis, the maximum torque value and minimum torque value under this motion state, detection sensitivity and the data after filtering, identify whether each axis occurs collision;Advantages are that the collision detection of mechanical hand and external environment can be realized without changing the structure of mechanical hand and without increasing the manufacturing cost of mechanical hand, and detection precision is high.

Description

Technical Field

[0001] This invention relates to a robot arm for injection molding machines, and more particularly to a collision detection method for a robot arm for injection molding machines. Background Technology

[0002] With the rapid development of the plastics industry, injection molding machine robots have been increasingly widely used. Injection molding machine robots primarily replace workers in the production process to complete material loading and unloading tasks. They can enter the mold cavity of the injection molding machine according to the control system's instructions to complete designated tasks. They are characterized by flexible structure and strong adaptability, and are suitable for high-repetition, high-intensity, and high-precision operations. In actual production processes, injection molding machine robots may collide with the external environment, potentially causing damage to the injection molding machine or the robot itself, and even threatening the safety of workers. Therefore, researching a collision detection method for injection molding machine robots is particularly necessary.

[0003] Currently, there is no relevant collision detection method in the field of injection molding machine robots. In the field of robotics, some commonly used collision detection methods involve installing force sensors at the robot joints. This method increases the hardware cost of the robot and changes its mechanical structure, making it unsuitable for mass production. Other methods involve establishing a robot dynamic model, predicting the theoretical torque of each joint through the dynamic model, and calculating the torque difference between the theoretical torque and the actual torque in real time. When the torque difference exceeds a threshold, a collision is considered to have occurred. This method requires the establishment of a dynamic model, which is relatively complex. Furthermore, because the dynamic model is not always accurate, the detection accuracy of this method is low. Summary of the Invention

[0004] The technical problem to be solved by the present invention is to provide a collision detection method for injection molding machine robots, which can realize the collision detection between the injection molding machine robot and the external environment without changing the mechanical structure of the injection molding machine robot or increasing the manufacturing cost of the injection molding machine robot, and has high detection accuracy, which can ensure the safe operation of the injection molding machine robot.

[0005] The technical solution adopted by the present invention to solve the above-mentioned technical problems is: a collision detection method for a robot arm of an injection molding machine, characterized by comprising the following steps:

[0006] Step 1: Configure the data acquisition module, calibration module, and collision detection module in the robot controller;

[0007] Step 2: During the fully automated operation of the first cycle of the robotic arm, the data acquisition module and calibration module work in each cycle. For the robotic arm's travel axis, left and right axes, and up and down axes, the data acquisition module collects the servo motor data of each axis and filters the data. The calibration module determines the motion state of each axis based on the filtered data and calibrates the maximum and minimum torque values ​​under that motion state. The specific process for the current cycle is as follows:

[0008] Step 2_1: Record the feedback torque and feedback speed of the corresponding servo motors read by the acquisition module from the servo drivers of each axis during the current cycle as t. fb and v fb ;

[0009] Step 2_2: For t respectively fb and v fb After filtering, the filtered feedback torque and filtered feedback speed are obtained, denoted as t. f and v f ;

[0010] Step 2_3: Within the current cycle, the calibration module calibrates according to v f v′ f Let T be the acceleration of the servo motors on each axis during the current cycle, denoted as a. f , Where the current cycle is the first cycle of the first fully automatic operation of the robotic arm, then in command v′ f =v f That is, v′ f The initial value is v f If the current cycle is not the first cycle of the first mode of fully automatic operation of the robotic arm, then v′ f This represents the filtered feedback speed obtained after filtering the feedback speed of the servo motor of each axis in the previous cycle, where T represents the duration of one cycle.

[0011] Step 2_4: Within the current cycle, the calibration module calibrates according to v f and a f To determine the motion state of each axis within the current cycle, specifically: when v f >0 and a f When v > 0, it is determined that the corresponding axis is in a positive acceleration state during the current cycle; when v f >0 and a f When v = 0, it is determined that the corresponding axis is in a positive uniform velocity state within the current cycle; when v f >0 and a f When v < 0, it is determined that the corresponding axis is in a positive deceleration state within the current cycle; when v f =0 and a fWhen v = 0, it is determined that the corresponding axis is in a stationary state within the current cycle; when v = 0, it is determined that the axis is in a stationary state within the current cycle. f <0 and a f When v < 0, it is determined that the corresponding axis is in a negative acceleration state within the current cycle; when v f <0 and a f When v = 0, it is determined that the corresponding axis is in a negative uniform velocity state within the current cycle; when v f <0 and a f When the value is greater than 0, it is determined that the corresponding axis is in a negative deceleration state during the current cycle;

[0012] Step 2_5: Within the current cycle, the calibration module calibrates the maximum and minimum torque values ​​under each axis's motion state, specifically as follows:

[0013] When the corresponding axis is in a positive acceleration state during the current cycle, calibrate the maximum and minimum torque values ​​under the positive acceleration state, denoted as t. pamax and t pamin If the current cycle is the torque calibration during the first positive acceleration state in the first mode of the fully automatic operation of the robotic arm, then let t pamax =t f t pamin =t f If the current cycle is not the torque calibration during the first positive acceleration state in the first mode of the fully automatic operation of the robotic arm, then if t f >t' pamax So update t pamax Let t be the value of t pamax =t f , keep t pamin The value of t remains unchanged, that is, let t pamin =t' pamin If t f <t' pamin So update t pamin Let t be the value of t pamin =t f , keep t pamax The value of t remains unchanged, that is, let t pamax =t' pamax If t' pamin ≤t f ≤t' pamax So keep t pamax and t pamin The value of t remains unchanged, that is, let t pamax =t' pamax t pamin =t' pamin ; where t' pamax This represents the maximum torque value during the last calibrated positive acceleration state, t' paminThis represents the minimum torque value under the last calibrated positive acceleration condition;

[0014] When the corresponding axis is in a positive uniform velocity state during the current cycle, calibrate the maximum and minimum torque values ​​under the positive uniform velocity state, denoted as t. pcmax and t pcmin If the current cycle is the torque calibration during the first positive uniform speed state in the first mode of fully automatic operation of the robotic arm, then let t pcmax =t f t pcmin =t f If the current cycle is not the torque calibration during the first positive uniform speed state in the first mode of fully automatic operation of the robotic arm, then if t f >t' pcmax So update t pcmax Let t be the value of t pcmax =t f , keep t pcmin The value of t remains unchanged, that is, let t pcmin =t' pcmin If t f <t' pcmin So update t pcmin Let t be the value of t pcmin =t f , keep t pcmax The value of t remains unchanged, that is, let t pcmax =t' pcmax If t' pcmin ≤t f ≤t' pcmax So keep t pcmax and t pcmin The value of t remains unchanged, that is, let t pcmax =t' pcmax t pcmin =t' pcmin ; where t' pcmax t' represents the maximum torque value under the previously calibrated positive constant velocity condition. pcmin This represents the minimum torque value under the previously calibrated positive constant velocity condition;

[0015] When the corresponding shaft is in a positive deceleration state during the current cycle, calibrate the maximum and minimum torque values ​​under the positive deceleration state, denoted as t. pdmax and t pdmin If the current cycle is the torque calibration during the first positive deceleration state in the first mode of the fully automatic operation of the robotic arm, then let t pdmax =t f t pdmin =t f If the current cycle is not the torque calibration during the first positive deceleration state in the first mode of the fully automatic operation of the robotic arm, then if tf >t' pdmax So update t pdmax Let t be the value of t pdmax =t f , keep t pdmin The value of t remains unchanged, that is, let t pdmin =t' pdmin If t f <t' pdmin So update t pdmin Let t be the value of t pdmin =t f , keep t pdmax The value of t remains unchanged, that is, let t pdmax =t' pdmax If t' pdmin ≤t f ≤t' pdmax So keep t pdmax and t pdmin The value of t remains unchanged, that is, let t pdmax =t' pdmax t pdmin =t' pdmin ; where t' pdmax This represents the maximum torque value under the last calibrated positive deceleration condition, t'. pdmin This indicates the minimum torque value under the last calibrated positive deceleration condition;

[0016] When the corresponding shaft is stationary during the current cycle, calibrate the maximum and minimum torque values ​​under stationary conditions, denoted as t. smax and t smin If the current cycle is the torque calibration during the first stationary state in the first phase of the fully automatic operation of the robotic arm, then let t smax =t f t smin =t f If the current cycle is not the torque calibration during the first stationary state in the first mode of the fully automatic operation of the robotic arm, then if t f >t' smax So update t smax Let t be the value of t smax =t f , keep t smin The value of t remains unchanged, that is, let t smin =t' smin If t f <t' smin So update t smin Let t be the value of t smin =t f , keep t smax The value of t remains unchanged, that is, let t smax =t' smaxIf t' smin ≤t f ≤t' smax So keep t smax and t smin The value of t remains unchanged, that is, let t smax =t' smax t smin =t' smin ; where t' smax t' represents the maximum torque value under the last calibrated static state. smin This represents the minimum torque value under the last calibrated static state;

[0017] When the corresponding axis is in a negative acceleration state during the current cycle, calibrate the maximum and minimum torque values ​​under the negative acceleration state, denoted as t. namax and t namin If the current cycle is the torque calibration during the first negative acceleration state in the first mode of fully automatic operation of the robotic arm, then let t namax =t f t namin =t f If the current cycle is not the torque calibration during the first negative acceleration state in the first mode of fully automatic operation of the robotic arm, then if t f >t' namax So update t namax Let t be the value of t namax =t f , keep t namin The value of t remains unchanged, that is, let t namin =t' namin If t f <t' namin So update t namin Let t be the value of t namin =t f , keep t namax The value of t remains unchanged, that is, let t namax =t' namax If t' namin ≤t f ≤t' namax So keep t namax and t namin The value of t remains unchanged, that is, let t namax =t' namax t namin =t' namin ; where t' namax This represents the maximum torque value under the last calibrated negative acceleration condition, t' namin This represents the minimum torque value under the last calibrated negative acceleration condition;

[0018] When the corresponding axis is in a negative uniform velocity state during the current cycle, calibrate the maximum and minimum torque values ​​under the negative uniform velocity state, denoted as t. ncmax and t ncmin If the current cycle is the torque calibration during the first negative uniform speed state in the first mode of fully automatic operation of the robotic arm, then let t ncmax =t f t ncmin =t f If the current cycle is not the torque calibration during the first negative uniform speed state in the first mode of fully automatic operation of the robotic arm, then if t f >t' ncmax So update t ncmax Let t be the value of t ncmax =t f , keep t ncmin The value of t remains unchanged, that is, let t ncmin =t' ncmin If t f <t' ncmin So update t ncmin Let t be the value of t ncmin =t f , keep t ncmax The value of t remains unchanged, that is, let t ncmax =t' ncmax If t' ncmin ≤t f ≤t' ncmax So keep t ncmax and t ncmin The value of t remains unchanged, that is, let t ncmax =t' ncmax t ncmin =t' ncmin ; where t' ncmax t' represents the maximum torque value under the previous calibration in the negative uniform velocity state. ncmin This represents the minimum torque value under the previous calibrated negative constant velocity condition;

[0019] When the corresponding shaft is in a negative deceleration state during the current cycle, calibrate the maximum and minimum torque values ​​under the negative deceleration state, denoted as t. ndmax and t ndmin If the current cycle is the torque calibration during the first negative deceleration state in the first mode of fully automatic operation of the robotic arm, then let t ndmax =t f t ndmin =t f If the current cycle is not the torque calibration during the first negative deceleration state in the first mode of fully automatic operation of the robotic arm, then if t f >t' ndmax So update t ndmaxLet t be the value of t ndmax =t f , keep t ndmin The value of t remains unchanged, that is, let t ndmin =t' ndmin If t f <t' ndmin So update t ndmin Let t be the value of t ndmin =t f , keep t ndmax The value of t remains unchanged, that is, let t ndmax =t' ndmax If t' ndmin ≤t f ≤t' ndmax So keep t ndmax and t ndmin The value of t remains unchanged, that is, let t ndmax =t' ndmax t ndmin =t' ndmin ; where t' ndmax This represents the maximum torque value under the last calibrated negative deceleration condition, t'. ndmin This indicates the minimum torque value under the last calibrated negative deceleration condition;

[0020] Step 3: During the first fully automated operation of the robotic arm, the calibration module calibrates the speed delay between the feedback speed and the command speed of the servo motors for the robotic arm's travel axis, left and right axes, and up and down axes, denoted as Δt, where Δt = n × T, and n represents the command speed v detected during the movement of the corresponding axis when the robotic arm controller executes the first motion segment command of the corresponding axis. c The difference in the number of cycles between when the speed reaches uniformity and when the filter feedback speed reaches the same uniform speed;

[0021] Step 4: During the fully automated operation of any module after the completion of the first module, the data acquisition module and collision detection module work in each cycle. For the robot's travel axis, left and right axes, and up and down axes, the data acquisition module collects data from the servo motors of each axis and filters the data. The collision detection module predicts the actual speed and acceleration based on the speed delay of the servo motors of each axis and the command speed of the corresponding axis's motion segment command, and determines the motion state of each axis. Then, based on the motion state of each axis, the maximum and minimum torque values ​​under that motion state, the detection sensitivity, and the filtered data, it identifies whether a collision has occurred on each axis. The specific process for the current cycle is as follows:

[0022] Step 4_1: Record the feedback torque and feedback speed of the servo motor of the corresponding axis, which are read by the acquisition module from the servo driver of each axis during the current cycle, as t. fb and v fb ;

[0023] Step 4_2: For t respectively fb and v fb After filtering, the filtered feedback torque and filtered feedback speed are obtained, denoted as t. f and v f ;

[0024] Step 4_3: Within the current cycle, the collision detection module predicts the actual speed of the servo motor of the corresponding axis within the current cycle based on the speed delay between the feedback speed and the command speed of the servo motor of each axis and the command speed of the robot controller executing the motion segment command of the corresponding axis within the current cycle. This speed is denoted as v. r v r =v c (t-Δt), where v c (t-Δt) represents the time offset, velocity delay Δt, which is then input into v. c The velocity value obtained from (t), v c (t) represents the command speed at which the robot controller executes the motion segment command for the corresponding axis within the current cycle;

[0025] Step 4_4: Within the current cycle, the collision detection module calculates the actual acceleration of the corresponding axis's servo motor predicted within the current cycle based on the predicted actual speed of each axis's servo motor within the current cycle, denoted as a. r , Wherein, if the current period is the first period, then let v' r =v r If the current period is not the first period, then v' r This represents the actual acceleration of the servo motor on the corresponding axis predicted in the previous cycle;

[0026] Steps 4-5: Within the current cycle, the collision detection module determines the collision detection value based on v. r and a r To determine the motion state of each axis within the current cycle, specifically: when v r >0 and a r When v > 0, it is determined that the corresponding axis is in a positive acceleration state during the current cycle; when v r >0 and a r When v = 0, it is determined that the corresponding axis is in a positive uniform velocity state within the current cycle; when v r >0 and a r When v < 0, it is determined that the corresponding axis is in a positive deceleration state within the current cycle; when v r =0 and a r When v = 0, it is determined that the corresponding axis is in a stationary state within the current cycle; when v = 0, it is determined that the axis is in a stationary state within the current cycle. r <0 and a rWhen v < 0, it is determined that the corresponding axis is in a negative acceleration state within the current cycle; when v r <0 and a r When v = 0, it is determined that the corresponding axis is in a negative uniform velocity state within the current cycle; when v r <0 and a r When the value is greater than 0, it is determined that the corresponding axis is in a negative deceleration state during the current cycle;

[0027] Steps 4-6: Within the current cycle, the collision detection module determines the motion state of each axis, the maximum and minimum torque values ​​under that motion state, the detection sensitivity, and t within the current cycle. f It identifies whether a collision has occurred on the corresponding axis within the current cycle, specifically:

[0028] If the corresponding axis is in a positive acceleration state during the current cycle, and if t is satisfied... f >t pamax +t s If a collision occurs on the corresponding axis within the current cycle, the robot arm will stop operating to achieve collision safety protection; if t is satisfied... f <t pamin -t s If the condition is true, a collision is detected on the corresponding axis within the current cycle, and the robot stops operating to achieve collision safety protection; otherwise, if the condition is not one of the two cases mentioned above, no collision is detected on the corresponding axis within the current cycle; where t s Indicates detection sensitivity;

[0029] If the corresponding axis is in a positive uniform velocity state during the current period, and if t f >t pcmax +t s If a collision occurs on the corresponding axis within the current cycle, the robot arm will stop operating to achieve collision safety protection; if t is satisfied... f <t pcmin -t s If the collision occurs, the robot arm stops operating to achieve collision safety protection; if the situation is different from the above two cases, the robot arm stops operating to achieve collision safety protection.

[0030] If the corresponding axis is in a positive deceleration state during the current cycle, and if t is satisfied... f >t pdmax +t s If a collision occurs on the corresponding axis within the current cycle, the robot arm will stop operating to achieve collision safety protection; if t is satisfied... f <t pdmin -t sIf the collision occurs, the robot arm stops operating to achieve collision safety protection; if the situation is different from the above two cases, the robot arm stops operating to achieve collision safety protection.

[0031] If the corresponding axis is stationary during the current period, and if t is satisfied... f >t smax +t s If a collision occurs on the corresponding axis within the current cycle, the robot arm will stop operating to achieve collision safety protection; if t is satisfied... f <t smin -t s If the collision occurs, the robot arm stops operating to achieve collision safety protection; if the situation is different from the above two cases, the robot arm stops operating to achieve collision safety protection.

[0032] If the corresponding axis is in a negative acceleration state during the current cycle, and if t is satisfied... f >t namax +t s If a collision occurs on the corresponding axis within the current cycle, the robot arm will stop operating to achieve collision safety protection; if t is satisfied... f <t namin -t s If the collision occurs, the robot arm stops operating to achieve collision safety protection; if the situation is different from the above two cases, the robot arm stops operating to achieve collision safety protection.

[0033] If the corresponding axis is in a negative uniform velocity state during the current period, and if t f >t ncmax +t s If a collision occurs on the corresponding axis within the current cycle, the robot arm will stop operating to achieve collision safety protection; if t is satisfied... f <t ncmin -t s If the collision occurs, the robot arm stops operating to achieve collision safety protection; if the situation is different from the above two cases, the robot arm stops operating to achieve collision safety protection.

[0034] When the corresponding axis is in a negative deceleration state during the current cycle, if t is satisfied... f >t ndmax +t s If a collision occurs on the corresponding axis within the current cycle, the robot arm will stop operating to achieve collision safety protection; if t is satisfied... f <t ndmin -t sIf the collision occurs, the robot arm stops operating to achieve collision safety protection; otherwise, if the collision does not occur, the robot arm stops operating to achieve collision safety protection.

[0035] In steps 2_2 and 4_2, t f =α×t fb +(1-α)t′ f v f =β×v fb +(1-β)v′ f Where α represents the torque filtering parameter, 0≤α≤1, and β represents the speed filtering parameter, 0≤β≤1. If the current cycle is the first cycle, then at t f =α×t fb +(1-α)t′ f Command t′ f =t fb In v f =β×v fb +(1-β)v′ f command v′ f =v fb If the current period is not the first period, then t′ f This represents the filtered feedback torque obtained after filtering the feedback torque of the servo motors on each axis in the previous cycle. If the current cycle is not the first cycle, then v′ f This represents the filtered feedback speed obtained after filtering the feedback speed of the servo motors on each axis in the previous cycle.

[0036] In steps 4-6, high sensitivity is required when t s =t max ×5%, t when general sensitivity is required s =t max ×10%, when low sensitivity is required t s =t max ×15%; of which, t max This indicates the rated torque value of the motor.

[0037] In step 4_3, when the robot controller executes the motion segment command for the corresponding axis within the current cycle, the command speed when the corresponding axis is in a non-stationary state is described as follows: The command velocity description when the corresponding axis is stationary is: v c (t) = 0, where t0 represents the moment when the motion segment being executed in the current cycle is started by the robot controller, and t1 represents the moment when the motion segment being executed in the current cycle is completed by the robot controller, t1 = t0 + 2T a +T c t is the time variable, vu This represents the velocity during a uniform speed segment, the duration of which is T. c , l represents the length of the motion segment, the symbol "||" indicates the absolute value, and the durations of both the acceleration and deceleration segments are T. a , a user Acceleration parameters set by the user.

[0038] Compared with the prior art, the advantages of the present invention are as follows:

[0039] 1) By setting up a data acquisition module, a calibration module, and a collision detection module in the robot controller, the data acquisition module collects data from the servo motors of each axis and filters the data during each fully automatic operation. During the first fully automatic operation, the calibration module determines the motion state of each axis based on the filtered data and calibrates the maximum and minimum torque values ​​under that motion state. The calibration module also calibrates the speed delay between the feedback speed and the command speed of each axis. In each subsequent fully automatic operation, the collision detection module predicts the actual speed and actual acceleration based on the command speed and speed delay of each axis and determines the motion state of each axis. Then, based on the motion state of each axis, the maximum and minimum torque values ​​under that motion state, the detection sensitivity, and the filtered data, it identifies whether a collision has occurred on each axis. When a collision occurs, the robot stops operating, protecting the robot from the external environment.

[0040] 2) The method of the present invention is simple to implement, has high detection accuracy, and is suitable for actual production environments.

[0041] 3) The method of the present invention is also relatively easy to maintain and upgrade.

[0042] 4) The method of the present invention does not change the mechanical structure of the robotic arm, nor does it increase the manufacturing cost of the robotic arm. Attached Figure Description

[0043] Figure 1 This is a schematic diagram of the robotic arm of an injection molding machine;

[0044] Figure 2 This is a block diagram illustrating the overall implementation of the method of the present invention;

[0045] Figure 3 This is a schematic diagram illustrating the process of calibrating the maximum and minimum torque values ​​under the motion state and calibrating the speed delay between the feedback speed and the command speed of the servo motor in the method of the present invention.

[0046] Figure 4 This is a schematic diagram illustrating the collision detection implementation process in the method of the present invention. Detailed Implementation

[0047] The present invention will be further described in detail below with reference to the accompanying drawings and embodiments.

[0048] like Figure 1 As shown, the robot arm of the injection molding machine 8 has a traveling axis 5, left and right axes 10, and up and down axes 7. The traveling axis 5 is mounted on the robot arm traveling axis guide rail 6 and can move on the robot arm traveling axis guide rail 6. The left and right axes 10 are mounted on the robot arm left and right axis guide rail 9 and can move on the robot arm left and right axis guide rail 9. The up and down axes 7 are mounted on the left and right axes 10 and can move on the left and right axes 10. The traveling axis servo driver, the left and right axis servo driver, the up and down axis servo driver (collectively referred to as servo driver 2) and the robot arm controller 4 are installed in the robot arm control cabinet 1. The robot arm control cabinet 1 is installed on the robot arm. The robot arm controller 4 can read data from the servo driver 2 through the fieldbus 3.

[0049] The present invention proposes a collision detection method for a robot arm in an injection molding machine, the overall implementation block diagram of which is shown below. Figure 2 As shown, it includes the following steps:

[0050] Step 1: Configure the data acquisition module, calibration module, and collision detection module in the robot controller.

[0051] Step 2: During the fully automated operation of the first cycle of the robotic arm (each cycle of the fully automated robotic arm executes a complete pick-up and place-up process), the data acquisition module and calibration module work in each cycle. For the robotic arm's travel axis, left and right axes, and up and down axes, the data acquisition module collects data from the servo motors of each axis and filters the data. The calibration module determines the motion state of each axis based on the filtered data and calibrates the maximum and minimum torque values ​​under that motion state. The specific process for the current cycle is as follows:

[0052] Step 2_1: The feedback torque and feedback speed of the corresponding servo motors read by the acquisition module from the servo drivers of each axis via the fieldbus during the current cycle are recorded as t. fb and v fb The data acquisition module collects data once per cycle.

[0053] Step 2_2: For t respectively fb and v fb After filtering, the filtered feedback torque and filtered feedback speed are obtained, denoted as t. f and v f .

[0054] In this embodiment, in step 2_2, t f =α×t fb +(1-α)t′ f v f =β×vfb +(1-β)v′ f Where α represents the torque filtering parameter, 0≤α≤1, and β represents the speed filtering parameter, 0≤β≤1. In this embodiment, α = 0.5 and β = 0.5. The smaller the value of α, the smoother the torque filtering; the larger the value of α, the better the real-time performance of the torque filtering. The smaller the value of β, the smoother the speed filtering; the larger the value of β, the better the real-time performance of the speed filtering. If the current period is the first period, then at t... f =α×t fb +(1-α)t′ f Command t′ f =t fb In v f =β×v fb +(1-β)v′ f command v′ f =v fb , i.e. t′ f The initial value is t fb , v′ f The initial value is v fb If the current period is not the first period, then t′ f This represents the filtered feedback torque obtained after filtering the feedback torque of the servo motors on each axis in the previous cycle. If the current cycle is not the first cycle, then v′ f This represents the filtered feedback speed obtained after filtering the feedback speed of the servo motors on each axis in the previous cycle.

[0055] Step 2_3: Within the current cycle, the calibration module calibrates according to v f v′ f Let T be the acceleration of the servo motors on each axis during the current cycle, denoted as a. f , Where the current cycle is the first cycle of the first fully automatic operation of the robotic arm, then in command v′ f =v f That is, v′ f The initial value is v f If the current cycle is not the first cycle of the first mode of fully automatic operation of the robotic arm, then v′ f This represents the filtered feedback speed obtained after filtering the feedback speed of the servo motor of each axis in the previous cycle. T represents the duration of one cycle, which is generally 2ms.

[0056] Step 2_4: Within the current cycle, the calibration module calibrates according to v f and a f To determine the motion state of each axis within the current cycle, specifically: when v f >0 and a fWhen v > 0, it is determined that the corresponding axis is in a positive acceleration state during the current cycle; when v f >0 and a f When v = 0, it is determined that the corresponding axis is in a positive uniform velocity state within the current cycle; when v f >0 and a f When v < 0, it is determined that the corresponding axis is in a positive deceleration state within the current cycle; when v f =0 and a f When v = 0, it is determined that the corresponding axis is in a stationary state within the current cycle; when v = 0, it is determined that the axis is in a stationary state within the current cycle. f <0 and a f When v < 0, it is determined that the corresponding axis is in a negative acceleration state within the current cycle; when v f <0 and a f When v = 0, it is determined that the corresponding axis is in a negative uniform velocity state within the current cycle; when v f <0 and a f When the value is greater than 0, it is determined that the corresponding axis is in a negative deceleration state during the current cycle.

[0057] Step 2_5: Within the current cycle, the calibration module calibrates the maximum and minimum torque values ​​under each axis's motion state, such as... Figure 3 As shown, specifically:

[0058] When the corresponding axis is in a positive acceleration state during the current cycle, calibrate the maximum and minimum torque values ​​under the positive acceleration state, denoted as t. pamax and t pamin If the current cycle is the torque calibration during the first positive acceleration state in the first mode of the fully automatic operation of the robotic arm, then let t pamax =t f t pamin =t f If the current cycle is not the torque calibration during the first positive acceleration state in the first mode of fully automatic operation of the robotic arm (i.e., not the torque calibration during the first positive acceleration state), then if t f >t' pamax So update t pamax Let t be the value of t pamax =t f , keep t pamin The value of t remains unchanged, that is, let t pamin =t' pamin If t f <t' pamin So update t pamin Let t be the value of t pamin =t f , keep t pamax The value of t remains unchanged, that is, let t pamax =t' pamax If t' pamin≤t f ≤t' pamax So keep t pamax and t pamin The value of t remains unchanged, that is, let t pamax =t' pamax t pamin =t' pamin ; where t' pamax This represents the maximum torque value during the last calibrated positive acceleration state, t' pamin This indicates the minimum torque value under the last calibrated positive acceleration condition.

[0059] When the corresponding axis is in a positive uniform velocity state during the current cycle, calibrate the maximum and minimum torque values ​​under the positive uniform velocity state, denoted as t. pcmax and t pcmin If the current cycle is the torque calibration during the first positive uniform speed state in the first mode of fully automatic operation of the robotic arm, then let t pcmax =t f t pcmin =t f If the current cycle is not the first torque calibration under the positive uniform speed state during the first mode of fully automatic operation of the robotic arm (i.e., not the first torque calibration under the positive uniform speed state), then if t f >t' pcmax So update t pcmax Let t be the value of t pcmax =t f , keep t pcmin The value of t remains unchanged, that is, let t pcmin =t' pcmin If t f <t' pcmin So update t pcmin Let t be the value of t pcmin =t f , keep t pcmax The value of t remains unchanged, that is, let t pcmax =t' pcmax If t' pcmin ≤t f ≤t' pcmax So keep t pcmax and t pcmin The value of t remains unchanged, that is, let t pcmax =t' pcmax t pcmin =t' pcmin ; where t' pcmax t' represents the maximum torque value under the previously calibrated positive constant velocity condition. pcmin This represents the minimum torque value under the previously calibrated positive constant speed condition.

[0060] When the corresponding shaft is in a positive deceleration state during the current cycle, calibrate the maximum and minimum torque values ​​under the positive deceleration state, denoted as t. pdmax and t pdmin If the current cycle is the torque calibration during the first positive deceleration state in the first mode of the fully automatic operation of the robotic arm, then let t pdmax =t f t pdmin =t f If the current cycle is not the torque calibration during the first positive deceleration state in the first mode of fully automatic operation of the robotic arm (i.e., not the torque calibration during the first positive deceleration state), then if t f >t' pdmax So update t pdmax Let t be the value of t pdmax =t f , keep t pdmin The value of t remains unchanged, that is, let t pdmin =t' pdmin If t f <t' pdmin So update t pdmin Let t be the value of t pdmin =t f , keep t pdmax The value of t remains unchanged, that is, let t pdmax =t' pdmax If t' pdmin ≤t f ≤t' pdmax So keep t pdmax and t pdmin The value of t remains unchanged, that is, let t pdmax =t' pdmax t pdmin =t' pdmin ; where t' pdmax This represents the maximum torque value under the last calibrated positive deceleration condition, t'. pdmin This indicates the minimum torque value under the last calibrated positive deceleration condition.

[0061] When the corresponding shaft is stationary during the current cycle, calibrate the maximum and minimum torque values ​​under stationary conditions, denoted as t. smax and t smin If the current cycle is the torque calibration during the first stationary state in the first phase of the fully automatic operation of the robotic arm, then let t smax =t f t smin =t f If the current cycle is not the torque calibration during the first stationary state in the first phase of the fully automatic operation of the robotic arm (i.e., not the first torque calibration during the stationary state), then if t f >t' smaxSo update t smax Let t be the value of t smax =t f , keep t smin The value of t remains unchanged, that is, let t smin =t' smin If t f <t' smin So update t smin Let t be the value of t smin =t f , keep t smax The value of t remains unchanged, that is, let t smax =t' smax If t' smin ≤t f ≤t' smax So keep t smax and t smin The value of t remains unchanged, that is, let t smax =t' smax t smin =t' smin ; where t' smax t' represents the maximum torque value under the last calibrated static state. smin This represents the minimum torque value under the last calibrated static state.

[0062] When the corresponding axis is in a negative acceleration state during the current cycle, calibrate the maximum and minimum torque values ​​under the negative acceleration state, denoted as t. namax and t namin If the current cycle is the torque calibration during the first negative acceleration state in the first mode of fully automatic operation of the robotic arm, then let t namax =t f t namin =t f If the current cycle is not the torque calibration under the first negative acceleration state during the first mode of fully automatic operation of the robotic arm (i.e., not the torque calibration under the first negative acceleration state), then if t f >t' namax So update t namax Let t be the value of t namax =t f , keep t namin The value of t remains unchanged, that is, let t namin =t' namin If t f <t' namin So update t namin Let t be the value of t namin =t f , keep t namax The value of t remains unchanged, that is, let t namax =t' namax If t'namin ≤t f ≤t' namax So keep t namax and t namin The value of t remains unchanged, that is, let t namax =t' namax t namin =t' namin ; where t' namax This represents the maximum torque value under the last calibrated negative acceleration condition, t' namin This indicates the minimum torque value under the last calibrated negative acceleration condition.

[0063] When the corresponding axis is in a negative uniform velocity state during the current cycle, calibrate the maximum and minimum torque values ​​under the negative uniform velocity state, denoted as t. ncmax and t ncmin If the current cycle is the torque calibration during the first negative uniform speed state in the first mode of fully automatic operation of the robotic arm, then let t ncmax =t f t ncmin =t f If the current cycle is not the torque calibration under the first negative uniform speed state during the first mode of fully automatic operation of the robotic arm (i.e., not the first torque calibration under the negative uniform speed state), then if t f >t' ncmax So update t ncmax Let t be the value of t ncmax =t f , keep t ncmin The value of t remains unchanged, that is, let t ncmin =t' ncmin If t f <t' ncmin So update t ncmin Let t be the value of t ncmin =t f , keep t ncmax The value of t remains unchanged, that is, let t ncmax =t' ncmax If t' ncmin ≤t f ≤t' ncmax So keep t ncmax and t ncmin The value of t remains unchanged, that is, let t ncmax =t' ncmax t ncmin =t' ncmin ; where t' ncmax t' represents the maximum torque value under the previous calibration in the negative uniform velocity state. ncmin This represents the minimum torque value under the previous calibrated negative constant velocity condition.

[0064] When the corresponding shaft is in a negative deceleration state during the current cycle, calibrate the maximum and minimum torque values ​​under the negative deceleration state, denoted as t. ndmax and t ndmin If the current cycle is the torque calibration during the first negative deceleration state in the first mode of fully automatic operation of the robotic arm, then let t ndmax =t f t ndmin =t f If the current cycle is not the torque calibration during the first negative deceleration state in the first mode of fully automatic operation of the robotic arm (i.e., not the torque calibration during the first negative deceleration state), then if t f >t' ndmax So update t ndmax Let t be the value of t ndmax =t f , keep t ndmin The value of t remains unchanged, that is, let t ndmin =t' ndmin If t f <t' ndmin So update t ndmin Let t be the value of t ndmin =t f , keep t ndmax The value of t remains unchanged, that is, let t ndmax =t' ndmax If t' ndmin ≤t f ≤t' ndmax So keep t ndmax and t ndmin The value of t remains unchanged, that is, let t ndmax =t' ndmax t ndmin =t' ndmin ; where t' ndmax This represents the maximum torque value under the last calibrated negative deceleration condition, t'. ndmin This indicates the minimum torque value under the last calibrated negative deceleration condition.

[0065] Step 3: As Figure 3 As shown, during the first fully automated operation of the robotic arm, the calibration module calibrates the speed delay between the feedback speed and the command speed of the servo motors for the robotic arm's travel axis, left and right axes, and up and down axes, denoted as Δt, where Δt = n × T, and n represents the command speed v detected during the movement of the corresponding axis when the robotic arm controller executes the first motion segment command of the corresponding axis. c The difference in the number of cycles between when the speed reaches uniformity and when the filter feedback speed reaches the same uniform speed.

[0066] In practical implementation, when the robot controller executes the first motion segment command for each axis, it begins cycle counting when the commanded speed reaches a constant speed. Cycle counting ends when the filtered feedback speed also reaches a constant speed. The product of the cycle difference (cycle count difference n) and the cycle duration T is the speed delay Δt between the feedback speed and the commanded speed. For example, assuming cycle counting begins when the commanded speed reaches 100 m / s, the filtered feedback speed might be 90 m / s. After 3 cycles, the filtered feedback speed also reaches 100 m / s, and cycle counting ends. The cycle duration T is 2 ms, then the speed delay Δt between the feedback speed and the commanded speed is 6 ms.

[0067] Step 4: During the fully automated operation of any module after the completion of the first module (i.e., starting from the second module), the data acquisition module and collision detection module work in each cycle. For the robot's travel axis, left and right axes, and up and down axes, the data acquisition module collects data from the servo motors of each axis and filters the data. The collision detection module predicts the actual speed and acceleration based on the speed delay of the servo motors of each axis and the command speed of the corresponding axis's motion segment, and determines the motion state of each axis. Then, based on the motion state of each axis, the maximum and minimum torque values ​​under that motion state, the detection sensitivity, and the filtered data, it identifies whether a collision has occurred on each axis. Figure 4 As shown, the specific process for the current cycle is as follows:

[0068] Step 4_1: The feedback torque and feedback speed of the corresponding servo motor of each axis, read by the acquisition module from the servo driver of each axis via the fieldbus during the current cycle, are recorded as t. fb and v fb The data acquisition module collects data once per cycle.

[0069] Step 4_2: For t respectively fb and v fb After filtering, the filtered feedback torque and filtered feedback speed are obtained, denoted as t. f and v f .

[0070] In this embodiment, in step 4_2, t f =α×t fb +(1-α)t′ f v f =β×v fb +(1-β)v′ fWhere α represents the torque filtering parameter, 0≤α≤1, and β represents the speed filtering parameter, 0≤β≤1. In this embodiment, α = 0.5 and β = 0.5. The smaller the value of α, the smoother the torque filtering; the larger the value of α, the better the real-time performance of the torque filtering. The smaller the value of β, the smoother the speed filtering; the larger the value of β, the better the real-time performance of the speed filtering. If the current period is the first period, then at t... f =α×t fb +(1-α)t′ f Command t′ f =t fb In v f =β×v fb +(1-β)v′ f command v′ f =v fb , i.e. t′ f The initial value is t fb , v′ f The initial value is v fb If the current period is not the first period, then t′ f This represents the filtered feedback torque obtained after filtering the feedback torque of the servo motors on each axis in the previous cycle. If the current cycle is not the first cycle, then v′ f This represents the filtered feedback speed obtained after filtering the feedback speed of the servo motors on each axis in the previous cycle.

[0071] Step 4_3: Within the current cycle, the collision detection module predicts the actual speed of the servo motor of the corresponding axis within the current cycle based on the speed delay between the feedback speed and the command speed of the servo motor of each axis and the command speed of the robot controller executing the motion segment command of the corresponding axis within the current cycle. This speed is denoted as v. r v r =v c (t-Δt), where v c (t-Δt) represents the time offset, velocity delay Δt, which is then input into v. c The velocity value obtained from (t), v c (t) represents the command speed at which the robot controller executes the motion segment command for the corresponding axis within the current cycle.

[0072] In this embodiment, in step 4_3, when the robot controller executes the motion segment command for the corresponding axis within the current cycle, the command speed when the corresponding axis is in a non-stationary state is described as follows: The command velocity description when the corresponding axis is stationary is: v c (t) = 0, where t0 represents the moment when the motion segment being executed in the current cycle is started by the robot controller, and t1 represents the moment when the motion segment being executed in the current cycle is completed by the robot controller, t1 = t0 + 2Ta +T c t is the time variable, v u The velocity v represents the velocity during the uniform motion segment. u User-defined, v u The value range is generally 0.1 mm / s to 5000 mm / s, and the duration of the uniform velocity segment is T. c , l represents the length of the motion segment, the symbol "||" indicates the absolute value, and the durations of both the acceleration and deceleration segments are T. a , a user The acceleration parameter a is set by the user. user The value range is generally 100 mm / s 2 ~30000mm / s 2 .

[0073] Step 4_4: Within the current cycle, the collision detection module calculates the actual acceleration of the corresponding axis's servo motor predicted within the current cycle based on the predicted actual speed of each axis's servo motor within the current cycle, denoted as a. r , Wherein, if the current period is the first period, then let v' r =v r If the current period is not the first period, then v' r This represents the actual acceleration of the servo motor on the corresponding axis predicted in the previous cycle.

[0074] Steps 4-5: Within the current cycle, the collision detection module determines the collision detection value based on v. r and a r To determine the motion state of each axis within the current cycle, specifically: when v r >0 and a r When v > 0, it is determined that the corresponding axis is in a positive acceleration state during the current cycle; when v r >0 and a r When v = 0, it is determined that the corresponding axis is in a positive uniform velocity state within the current cycle; when v r >0 and a r When v < 0, it is determined that the corresponding axis is in a positive deceleration state within the current cycle; when v r =0 and a r When v = 0, it is determined that the corresponding axis is in a stationary state within the current cycle; when v = 0, it is determined that the axis is in a stationary state within the current cycle. r <0 and a r When v < 0, it is determined that the corresponding axis is in a negative acceleration state within the current cycle; when v r <0 and a r When v = 0, it is determined that the corresponding axis is in a negative uniform velocity state within the current cycle; when v r <0 and a rWhen the value is greater than 0, it is determined that the corresponding axis is in a negative deceleration state during the current cycle.

[0075] Steps 4-6: Within the current cycle, the collision detection module determines the motion state of each axis, the maximum and minimum torque values ​​under that motion state, the detection sensitivity, and t within the current cycle. f It identifies whether a collision has occurred on the corresponding axis within the current cycle, specifically:

[0076] If the corresponding axis is in a positive acceleration state during the current cycle, and if t is satisfied... f >t pamax +t s If a collision occurs on the corresponding axis within the current cycle, the robot arm will stop operating to achieve collision safety protection; if t is satisfied... f <t pamin -t s If the condition is true, a collision is detected on the corresponding axis within the current cycle, and the robot stops operating to achieve collision safety protection; otherwise, if the condition is not one of the two cases mentioned above, no collision is detected on the corresponding axis within the current cycle; where t s Indicates the detection sensitivity.

[0077] If the corresponding axis is in a positive uniform velocity state during the current period, and if t f >t pcmax +t s If a collision occurs on the corresponding axis within the current cycle, the robot arm will stop operating to achieve collision safety protection; if t is satisfied... f <t pcmin -t s If the collision occurs, the robot arm stops operating to achieve collision safety protection; otherwise, if the collision does not occur, the robot arm stops operating to achieve collision safety protection.

[0078] If the corresponding axis is in a positive deceleration state during the current cycle, and if t is satisfied... f >t pdmax +t s If a collision occurs on the corresponding axis within the current cycle, the robot arm will stop operating to achieve collision safety protection; if t is satisfied... f <t pdmin -t s If the collision occurs, the robot arm stops operating to achieve collision safety protection; otherwise, if the collision does not occur, the robot arm stops operating to achieve collision safety protection.

[0079] If the corresponding axis is stationary during the current period, and if t is satisfied... f >t smax +t sIf a collision occurs on the corresponding axis within the current cycle, the robot arm will stop operating to achieve collision safety protection; if t is satisfied... f <t smin -t s If the collision occurs, the robot arm stops operating to achieve collision safety protection; otherwise, if the collision does not occur, the robot arm stops operating to achieve collision safety protection.

[0080] If the corresponding axis is in a negative acceleration state during the current cycle, and if t is satisfied... f >t namax +t s If a collision occurs on the corresponding axis within the current cycle, the robot arm will stop operating to achieve collision safety protection; if t is satisfied... f <t namin -t s If the collision occurs, the robot arm stops operating to achieve collision safety protection; otherwise, if the collision does not occur, the robot arm stops operating to achieve collision safety protection.

[0081] If the corresponding axis is in a negative uniform velocity state during the current period, and if t f >t ncmax +t s If a collision occurs on the corresponding axis within the current cycle, the robot arm will stop operating to achieve collision safety protection; if t is satisfied... f <t ncmin -t s If the collision occurs, the robot arm stops operating to achieve collision safety protection; otherwise, if the collision does not occur, the robot arm stops operating to achieve collision safety protection.

[0082] When the corresponding axis is in a negative deceleration state during the current cycle, if t is satisfied... f >t ndmax +t s If a collision occurs on the corresponding axis within the current cycle, the robot arm will stop operating to achieve collision safety protection; if t is satisfied... f <t ndmin -t s If the collision occurs, the robot arm stops operating to achieve collision safety protection; otherwise, if the collision does not occur, the robot arm stops operating to achieve collision safety protection.

[0083] In this embodiment, in steps 4-6, when high sensitivity is required, t s =t max ×5%, t when general sensitivity is required s =t max×10%, when low sensitivity is required t s =t max ×15%; of which, t max This indicates the rated torque value of the motor. Different motor models have different rated torque values, and users can select the detection sensitivity according to their actual production needs.

[0084] Practical application has shown that the method of this invention can accurately detect collisions between the robotic arm and the external environment.

Claims

1. A collision detection method for a robot arm of an injection molding machine, characterized in that... Includes the following steps: Step 1: Configure the data acquisition module, calibration module, and collision detection module in the robot controller; Step 2: During the fully automated operation of the first cycle of the robotic arm, the data acquisition module and calibration module work in each cycle. For the robotic arm's travel axis, left and right axes, and up and down axes, the data acquisition module collects the servo motor data of each axis and filters the data. The calibration module determines the motion state of each axis based on the filtered data and calibrates the maximum and minimum torque values ​​under that motion state. The specific process for the current cycle is as follows: Step 2_1: Record the feedback torque and feedback speed of the corresponding servo motors read by the acquisition module from the servo drivers of each axis during the current cycle as t. fb and v fb ; Step 2_2: For t respectively fb and v fb After filtering, the filtered feedback torque and filtered feedback speed are obtained, denoted as t. f and v f ; Step 2_3: Within the current cycle, the calibration module calibrates according to v f v′ f Let T be the acceleration of the servo motors on each axis during the current cycle, denoted as a. f , Where the current cycle is the first cycle of the first fully automatic operation of the robotic arm, then in command v′ f =v f That is, v′ f The initial value is v f If the current cycle is not the first cycle of the first mode of fully automatic operation of the robotic arm, then v′ f This represents the filtered feedback speed obtained after filtering the feedback speed of the servo motor of each axis in the previous cycle, where T represents the duration of one cycle. Step 2_4: Within the current cycle, the calibration module calibrates according to v f and a f To determine the motion state of each axis within the current cycle, specifically: when v f >0 and a f When v > 0, it is determined that the corresponding axis is in a positive acceleration state during the current cycle; when v f >0 and a f When v = 0, it is determined that the corresponding axis is in a positive uniform velocity state within the current cycle; when v f >0 and a f When v < 0, it is determined that the corresponding axis is in a positive deceleration state within the current cycle; when v f =0 and a f When v = 0, it is determined that the corresponding axis is in a stationary state within the current cycle; when v = 0, it is determined that the axis is in a stationary state within the current cycle. f <0 and a f When v < 0, it is determined that the corresponding axis is in a negative acceleration state within the current cycle; when v f <0 and a f When v = 0, it is determined that the corresponding axis is in a negative uniform velocity state within the current cycle; when v f <0 and a f When the value is greater than 0, it is determined that the corresponding axis is in a negative deceleration state during the current cycle; Step 2_5: Within the current cycle, the calibration module calibrates the maximum and minimum torque values ​​under each axis's motion state, specifically as follows: When the corresponding axis is in a positive acceleration state during the current cycle, calibrate the maximum and minimum torque values ​​under the positive acceleration state, denoted as t. pamax and t pamin If the current cycle is the torque calibration during the first positive acceleration state in the first mode of the fully automatic operation of the robotic arm, then let t pamax =t f t pamin =t f If the current cycle is not the torque calibration during the first positive acceleration state in the first mode of the fully automatic operation of the robotic arm, then if t f >t' pamax So update t pamax Let t be the value of t pamax =t f , keep t pamin The value of t remains unchanged, that is, let t pamin =t' pamin If t f <t' pamin So update t pamin Let t be the value of t pamin =t f , keep t pamax The value of t remains unchanged, that is, let t pamax =t' pamax If t' pamin ≤t f ≤t' pamax So keep t pamax and t pamin The value of t remains unchanged, that is, let t pamax =t' pamax t pamin =t' pamin ; where t' pamax This represents the maximum torque value during the last calibrated positive acceleration state, t' pamin This represents the minimum torque value under the last calibrated positive acceleration condition; When the corresponding axis is in a positive uniform velocity state during the current cycle, calibrate the maximum and minimum torque values ​​under the positive uniform velocity state, denoted as t. pcmax and t pcmin If the current cycle is the torque calibration during the first positive uniform speed state in the first mode of fully automatic operation of the robotic arm, then let t pcmax =t f t pcmin =t f If the current cycle is not the torque calibration during the first positive uniform speed state in the first mode of fully automatic operation of the robotic arm, then if t f >t' pcmax So update t pcmax Let t be the value of t pcmax =t f , keep t pcmin The value of t remains unchanged, that is, let t pcmin =t' pcmin If t f <t' pcmin So update t pcmin Let t be the value of t pcmin =t f , keep t pcmax The value of t remains unchanged, that is, let t pcmax =t' pcmax If t' pcmin ≤t f ≤t' pcmax So keep t pcmax and t pcmin The value of t remains unchanged, that is, let t pcmax =t' pcmax t pcmin =t' pcmin ; where t' pcmax t' represents the maximum torque value under the previously calibrated positive constant velocity condition. pcmin This represents the minimum torque value under the previously calibrated positive constant velocity condition; When the corresponding shaft is in a positive deceleration state during the current cycle, calibrate the maximum and minimum torque values ​​under the positive deceleration state, denoted as t. pdmax and t pdmin If the current cycle is the torque calibration during the first positive deceleration state in the first mode of the fully automatic operation of the robotic arm, then let t pdmax =t f t pdmin =t f If the current cycle is not the torque calibration during the first positive deceleration state in the first mode of the fully automatic operation of the robotic arm, then if t f >t' pdmax So update t pdmax Let t be the value of t pdmax =t f , keep t pdmin The value of t remains unchanged, that is, let t pdmin =t' pdmin If t f <t' pdmin So update t pdmin Let t be the value of t pdmin =t f , keep t pdmax The value of t remains unchanged, that is, let t pdmax =t' pdmax If t' pdmin ≤t f ≤t' pdmax So keep t pdmax and t pdmin The value of t remains unchanged, that is, let t pdmax =t' pdmax t pdmin =t' pdmin ; where t' pdmax This represents the maximum torque value under the last calibrated positive deceleration condition, t'. pdmin This indicates the minimum torque value under the last calibrated positive deceleration condition; When the corresponding shaft is stationary during the current cycle, calibrate the maximum and minimum torque values ​​under stationary conditions, denoted as t. smax and t smin If the current cycle is the torque calibration during the first stationary state in the first phase of the fully automatic operation of the robotic arm, then let t smax =t f t smin =t f If the current cycle is not the torque calibration during the first stationary state in the first mode of the fully automatic operation of the robotic arm, then if t f >t' smax So update t smax Let t be the value of t smax =t f , keep t smin The value of t remains unchanged, that is, let t smin =t' smin If t f <t' smin So update t smin Let t be the value of t smin =t f , keep t smax The value of t remains unchanged, that is, let t smax =t' smax If t' smin ≤t f ≤t' smax So keep t smax and t smin The value of t remains unchanged, that is, let t smax =t' smax t smin =t' smin ; where t' smax t' represents the maximum torque value under the last calibrated static state. smin This represents the minimum torque value under the last calibrated static state; When the corresponding axis is in a negative acceleration state during the current cycle, calibrate the maximum and minimum torque values ​​under the negative acceleration state, denoted as t. namax and t namin If the current cycle is the torque calibration during the first negative acceleration state in the first mode of fully automatic operation of the robotic arm, then let t namax =t f t namin =t f If the current cycle is not the torque calibration during the first negative acceleration state in the first mode of fully automatic operation of the robotic arm, then if t f >t' namax So update t namax Let t be the value of t namax =t f , keep t namin The value of t remains unchanged, that is, let t namin =t' namin If t f <t' namin So update t namin Let t be the value of t namin =t f , keep t namax The value of t remains unchanged, that is, let t namax =t' namax If t' namin ≤t f ≤t' namax So keep t namax and t namin The value of t remains unchanged, that is, let t namax =t' namax t namin =t' namin ; where t' namax This represents the maximum torque value under the last calibrated negative acceleration condition, t' namin This represents the minimum torque value under the last calibrated negative acceleration condition; When the corresponding axis is in a negative uniform velocity state during the current cycle, calibrate the maximum and minimum torque values ​​under the negative uniform velocity state, denoted as t. ncmax and t ncmin If the current cycle is the torque calibration during the first negative uniform speed state in the first mode of fully automatic operation of the robotic arm, then let t ncmax =t f t ncmin =t f If the current cycle is not the torque calibration during the first negative uniform speed state in the first mode of fully automatic operation of the robotic arm, then if t f >t' ncmax So update t ncmax Let t be the value of t ncmax =t f , keep t ncmin The value of t remains unchanged, that is, let t ncmin =t' ncmin If t f <t' ncmin So update t ncmin Let t be the value of t ncmin =t f , keep t ncmax The value of t remains unchanged, that is, let t ncmax =t' ncmax If t' ncmin ≤t f ≤t' ncmax So keep t ncmax and t ncmin The value of t remains unchanged, that is, let t ncmax =t' ncmax t ncmin =t' ncmin ; where t' ncmax t' represents the maximum torque value under the previous calibration in the negative uniform velocity state. ncmin This represents the minimum torque value under the previous calibrated negative constant velocity condition; When the corresponding shaft is in a negative deceleration state during the current cycle, calibrate the maximum and minimum torque values ​​under the negative deceleration state, denoted as t. ndmax and t ndmin If the current cycle is the torque calibration during the first negative deceleration state in the first mode of fully automatic operation of the robotic arm, then let t ndmax =t f t ndmin =t f If the current cycle is not the torque calibration during the first negative deceleration state in the first mode of fully automatic operation of the robotic arm, then if t f >t' ndmax So update t ndmax Let t be the value of t ndmax =t f , keep t ndmin The value of t remains unchanged, that is, let t ndmin =t' ndmin If t f <t' ndmin So update t ndmin Let t be the value of t ndmin =t f , keep t ndmax The value of t remains unchanged, that is, let t ndmax =t' ndmax If t' ndmin ≤t f ≤t' ndmax So keep t ndmax and t ndmin The value of t remains unchanged, that is, let t ndmax =t' ndmax t ndmin =t' ndmin ; where t' ndmax This represents the maximum torque value under the last calibrated negative deceleration condition, t'. ndmin This indicates the minimum torque value under the last calibrated negative deceleration condition; Step 3: During the first fully automated operation of the robotic arm, the calibration module calibrates the speed delay between the feedback speed and the command speed of the servo motors for the robotic arm's travel axis, left and right axes, and up and down axes, denoted as Δt, where Δt = n × T, and n represents the command speed v detected during the movement of the corresponding axis when the robotic arm controller executes the first motion segment command of the corresponding axis. c The difference in the number of cycles between when the speed reaches uniformity and when the filter feedback speed reaches the same uniform speed; Step 4: During the fully automated operation of any module after the completion of the first module, the data acquisition module and collision detection module work in each cycle. For the robot's travel axis, left and right axes, and up and down axes, the data acquisition module collects data from the servo motors of each axis and filters the data. The collision detection module predicts the actual speed and acceleration based on the speed delay of the servo motors of each axis and the command speed of the corresponding axis's motion segment command, and determines the motion state of each axis. Then, based on the motion state of each axis, the maximum and minimum torque values ​​under that motion state, the detection sensitivity, and the filtered data, it identifies whether a collision has occurred on each axis. The specific process for the current cycle is as follows: Step 4_1: Record the feedback torque and feedback speed of the corresponding servo motor of each axis, which are read by the acquisition module from the servo driver of each axis during the current cycle, as t. fb and v fb ; Step 4_2: For t respectively fb and v fb After filtering, the filtered feedback torque and filtered feedback speed are obtained, denoted as t. f and v f ; Step 4_3: Within the current cycle, the collision detection module predicts the actual speed of the servo motor of the corresponding axis within the current cycle based on the speed delay between the feedback speed and the command speed of the servo motor of each axis and the command speed of the robot controller executing the motion segment command of the corresponding axis within the current cycle. This speed is denoted as v. r v r =v c (t-Δt), where v c (t-Δt) represents the time offset, velocity delay Δt, which is then input into v. c The velocity value obtained from (t), v c (t) represents the command speed at which the robot controller executes the motion segment command for the corresponding axis within the current cycle; Step 4_4: Within the current cycle, the collision detection module calculates the actual acceleration of the corresponding axis's servo motor predicted within the current cycle based on the predicted actual speed of each axis's servo motor within the current cycle, denoted as a. r , Wherein, if the current period is the first period, then let v' r =v r If the current period is not the first period, then v' r This represents the actual acceleration of the servo motor on the corresponding axis predicted in the previous cycle; Steps 4-5: Within the current cycle, the collision detection module determines the collision detection value based on v. r and a r To determine the motion state of each axis within the current cycle, specifically: when v r >0 and a r When v > 0, it is determined that the corresponding axis is in a positive acceleration state during the current cycle; when v r >0 and a r When v = 0, it is determined that the corresponding axis is in a positive uniform velocity state within the current cycle; when v r >0 and a r When v < 0, it is determined that the corresponding axis is in a positive deceleration state within the current cycle; when v r =0 and a r When v = 0, it is determined that the corresponding axis is in a stationary state within the current cycle; when v = 0, it is determined that the axis is in a stationary state within the current cycle. r <0 and a r When v < 0, it is determined that the corresponding axis is in a negative acceleration state within the current cycle; when v r <0 and a r When v = 0, it is determined that the corresponding axis is in a negative uniform velocity state within the current cycle; when v r <0 and a r When the value is greater than 0, it is determined that the corresponding axis is in a negative deceleration state during the current cycle; Steps 4-6: Within the current cycle, the collision detection module determines the motion state of each axis, the maximum and minimum torque values ​​under that motion state, the detection sensitivity, and t within the current cycle. f It identifies whether a collision has occurred on the corresponding axis within the current cycle, specifically: If the corresponding axis is in a positive acceleration state during the current cycle, and if t is satisfied... f >t pamax +t s If a collision occurs on the corresponding axis within the current cycle, the robot arm will stop operating to achieve collision safety protection; if t is satisfied... f <t pamin -t s If the condition is true, a collision is detected on the corresponding axis within the current cycle, and the robot stops operating to achieve collision safety protection; if the condition is not one of the two cases mentioned above, no collision is detected on the corresponding axis within the current cycle; where t s Indicates detection sensitivity; If the corresponding axis is in a positive uniform velocity state during the current period, and if t f >t pcmax +t s If a collision occurs on the corresponding axis within the current cycle, the robot arm will stop operating to achieve collision safety protection; if t is satisfied... f <t pcmin -t s If the collision occurs, the robot arm stops operating to achieve collision safety protection; if the situation is different from the above two cases, the robot arm stops operating to achieve collision safety protection. If the corresponding axis is in a positive deceleration state during the current cycle, and if t is satisfied... f >t pdmax +t s If a collision occurs on the corresponding axis within the current cycle, the robot arm will stop operating to achieve collision safety protection; if t is satisfied... f <t pdmin -t s If the collision occurs, the robot arm stops operating to achieve collision safety protection; if the situation is different from the above two cases, the robot arm stops operating to achieve collision safety protection. If the corresponding axis is stationary during the current period, and if t is satisfied... f >t smax +t s If a collision occurs on the corresponding axis within the current cycle, the robot arm will stop operating to achieve collision safety protection; if t is satisfied... f <t smin -t s If the collision occurs, the robot arm stops operating to achieve collision safety protection; if the situation is different from the above two cases, the robot arm stops operating to achieve collision safety protection. If the corresponding axis is in a negative acceleration state during the current cycle, and if t is satisfied... f >t namax +t s If a collision occurs on the corresponding axis within the current cycle, the robot arm will stop operating to achieve collision safety protection; if t is satisfied... f <t namin -t s If the collision occurs, the robot arm stops operating to achieve collision safety protection; if the situation is different from the above two cases, the robot arm stops operating to achieve collision safety protection. If the corresponding axis is in a negative uniform velocity state during the current period, and if t f >t ncmax +t s If a collision occurs on the corresponding axis within the current cycle, the robot arm will stop operating to achieve collision safety protection; if t is satisfied... f <t ncmin -t s If the collision occurs, the robot arm stops operating to achieve collision safety protection; if the situation is different from the above two cases, the robot arm stops operating to achieve collision safety protection. When the corresponding axis is in a negative deceleration state during the current cycle, if t is satisfied... f >t ndmax +t s If a collision occurs on the corresponding axis within the current cycle, the robot arm will stop operating to achieve collision safety protection; if t is satisfied... f <t ndmin -t s If the collision occurs, the robot arm stops operating to achieve collision safety protection; otherwise, if the collision does not occur, the robot arm stops operating to achieve collision safety protection.

2. The collision detection method for a robot arm of an injection molding machine according to claim 1, characterized in that... In steps 2_2 and 4_2, t f =α×t fb +(1-α)t′ f v f =β×v fb +(1-β)v′ f Where α represents the torque filtering parameter, 0≤α≤1, and β represents the speed filtering parameter, 0≤β≤1. If the current cycle is the first cycle, then at t f =α×t fb +(1-α)t′ f Command t′ f =t fb In v f =β×v fb +(1-β)v′ f command v′ f =v fb If the current period is not the first period, then t′ f This represents the filtered feedback torque obtained after filtering the feedback torque of the servo motors on each axis in the previous cycle. If the current cycle is not the first cycle, then v′ f This represents the filtered feedback speed obtained after filtering the feedback speed of the servo motors on each axis in the previous cycle.

3. A collision detection method for an injection molding machine robot according to claim 1 or 2, characterized in that... In steps 4-6, high sensitivity is required when t s =t max ×5%, t when general sensitivity is required s =t max ×10%, when low sensitivity is required t s =t max ×15%; of which, t max This indicates the rated torque value of the motor.

4. The collision detection method for an injection molding machine robot as described in claim 3, characterized in that... In step 4_3, when the robot controller executes the motion segment command for the corresponding axis within the current cycle, the command speed when the corresponding axis is in a non-stationary state is described as follows: The command speed description when the corresponding axis is stationary is: v c (t) = 0, where t0 represents the moment when the motion segment being executed in the current cycle is started by the robot controller, and t1 represents the moment when the motion segment being executed in the current cycle is completed by the robot controller, t1 = t0 + 2T a +T c t is the time variable, v u This represents the velocity during a uniform motion segment, the duration of which is T. c , l represents the length of the motion segment, the symbol "||" indicates the absolute value, and the durations of both the acceleration and deceleration segments are T. a , a user Acceleration parameters set by the user.

Images