Industrial robot collision threshold value automatic calculation method based on servo driver

Abstract

The invention belongs to the technical field of industrial control, and provides an industrial robot collision threshold value automatic calculation method based on a servo driver, which is used for automatically finishing automatic setting of an industrial robot collision threshold value through the servo driver, and improving the working efficiency while realizing a collision detection function of an industrial robot. According to the method, firstly, motor position information and motor current information are collected through a servo driver, and the motor rotating speed, the motor acceleration and the motor torque current are obtained through calculation; then calculating to obtain theoretical moments and feedback moments of the first joint to the sixth joint and a difference value between the theoretical moments and the feedback moments; and finally, searching the maximum difference value of the theoretical torque and the feedback torque in the complete action period, and performing coefficient amplification on the maximum difference value to serve as a collision threshold value calculation result of the first joint to the sixth joint. Therefore, the collision threshold value of the industrial robot is automatically set through the servo driver, and the working efficiency is improved while the collision detection function of the industrial robot is achieved.

Description

Technical Field

[0001] The present invention belongs to the technical field of industrial control, relates to the collision detection technology of industrial robots, and specifically provides an automatic calculation method for the collision threshold value of industrial robots based on servo drivers. Background Art

[0002] Under normal circumstances, industrial robots execute actions repeatedly according to the taught movements. Each execution of an action has corresponding theoretical torque curves and feedback torque curves. The theoretical torque curves for each action are almost the same, the feedback torque curves are also almost the same, and the theoretical torque and the feedback torque curves are relatively close. However, in the case of a collision, the difference between the theoretical torque and the feedback torque will increase sharply. The dynamics of the robot reflects the relationship between the joint torques and joint movements of the industrial robot, and the required torque can be calculated based on the current motion state of the robot joints, which is then used as an important basis for dynamic collision detection. For the collision detection of industrial robots, kinematic and dynamic analyses can obtain information such as the positions, velocities, and torques of each joint of the robot in a certain state, facilitating the judgment of the working state of the industrial robot and combining the actual torque feedback to determine the occurrence of a collision event.

[0003] Industrial robots usually have 6 joints and 6 degrees of freedom; in order to realize the automatic calculation of the collision threshold value of industrial robots, first, the theoretical torque of each joint needs to be calculated through robot dynamics and robot parameters, and second, the feedback torque of each joint needs to be calculated through parameters such as motor current. During a complete action cycle, the feedback torque and theoretical torque of the industrial robot are both related to the executed action and the end load; when the industrial robot is working normally, the feedback torque curve and the theoretical torque curve are relatively close, with a reasonable error, as Figure 1 shown; when a collision occurs, the deviation between the feedback torque and the theoretical torque will increase sharply, as Figure 2 shown; therefore, the collision detection of industrial robots can be realized through the deviation value between the feedback torque and the theoretical torque. The setting of the collision threshold value is particularly crucial. If it is set too small, false alarms may occur because there is a reasonable error between the theoretical torque and the feedback torque for each action of the industrial robot; in addition, for soft objects, if the collision threshold value is set too small, the collision cannot be detected; if the collision threshold value is set too large, the alarm cannot be given in time, which is not conducive to protecting the equipment. Currently, the collision threshold value is usually set manually, with great inaccuracy. Therefore, the present invention provides an automatic calculation method for the collision threshold value of industrial robots based on servo drivers to improve work efficiency and reduce work difficulty. Summary of the Invention

[0004] The object of the present invention is to provide an automatic calculation method for the collision threshold value of an industrial robot based on a servo driver, so as to automatically complete the automatic setting of the collision threshold value of the industrial robot through the servo driver, realizing the collision detection function of the industrial robot while improving the working efficiency.

[0005] To achieve the above object, the technical solution adopted by the present invention is as follows:

[0006] An automatic calculation method for the collision threshold value of an industrial robot based on a servo driver, comprising the following steps:

[0007] Step 1. The servo driver establishes communication with the main controller and enters the enabled state;

[0008] Step 2. The servo driver receives the automatic save instruction for the collision detection threshold value sent by the main controller and starts calculating the collision detection threshold value;

[0009] Step 2.1. Collect the motor position information and the motor current information;

[0010] Step 2.2. Calculate the motor speed and the motor acceleration based on the collected motor position information and the control period;

[0011] Step 2.3. Calculate the motor torque current based on the collected motor current information and the motor position information;

[0012] Step 2.4. According to the motor position information, speed information, acceleration information and the physical parameters of the robot, calculate the theoretical torques of the first joint to the sixth joint by using robot dynamics ~ ;

[0013] Step 2.5. Calculate the feedback torques of the first joint to the sixth joint according to the motor torque current ~ ;

[0014] Step 2.6. Calculate the differences between the theoretical torques and the feedback torques of the first joint to the sixth joint respectively ~ ;

[0015] Step 2.7. Record the differences between the theoretical torques and the feedback torques of the first joint to the sixth joint in each control period within the complete action cycle, and search for the corresponding maximum value ~ ; Multiply ~ by the same amplification factor for amplification, and the result is expressed as ~ , and ~ Calculation results of the collision threshold values for the first to sixth joints respectively;

[0016] Step 3. The servo driver receives the instruction for automatically saving the collision parameter threshold and saves the calculation results of the collision threshold values for the first to sixth joints to the non-volatile memory, and sends a success flag to the main controller;

[0017] Step 4. If the main controller receives the success flag within the preset time window, the automatic calculation of the collision threshold value of the industrial robot is completed; otherwise, an alarm prompt is given.

[0018] Furthermore, in step 2.1, the acquisition of the motor position information is completed by an encoder installed at the motor end, and the motor current information is collected from a sampling resistor or a Hall sensor set in the motor current channel of the servo driver.

[0019] Furthermore, in step 2.2, the motor speed is the first derivative of the position information, and the motor acceleration is the second derivative of the position information.

[0020] Furthermore, in step 2.3, first, the α-axis current and the β-axis current are calculated based on the motor current information, specifically:

[0021] , ;

[0022] where and represent the α-axis current and the β-axis current respectively; , , are the U, V, and W phase currents of the motor;

[0023] Then, the equivalent torque current of the motor is calculated based on the α-axis current, the β-axis current, and the motor position information , specifically:

[0024] , ;

[0025] where represents the electrical angle of the motor, represents the electrical angle offset, represents the motor position information, and P represents the number of pole pairs of the motor.

[0026] Furthermore, in step 2.5, the calculation process of the feedback torque is:

[0027] ;

[0028] where represents the feedback torque, Indicates the motor torque current, Indicates the rated torque of the motor, Indicates the reduction ratio of the motor, Indicates the direction of rotation.

[0029] Further, in step 2.7, the amplification factor is 1.2.

[0030] Further, in step 4, the preset time window is 2 s.

[0031] Based on the above technical solution, the beneficial effects of the present invention are as follows:

[0032] The present invention provides an automatic calculation method for the collision threshold value of an industrial robot based on a servo driver, which automatically completes the automatic setting of the collision threshold value of the industrial robot through the servo driver, improves work efficiency, and avoids the problem of mismatch of the collision threshold value caused by manual setting. Description of the Drawings

[0033] Figure 1 It is the feedback torque curve and the theoretical torque curve diagram when the industrial robot is working normally.

[0034] Figure 2 It is the feedback torque curve and the theoretical torque curve diagram when the industrial robot collides.

[0035] Figure 3 It is the flow schematic diagram of the automatic calculation method for the collision threshold value of the industrial robot based on the servo driver in the present invention. Detailed Embodiment

[0036] To make the purpose, technical solution and beneficial effects of the present invention clearer and more understandable, the present invention will be further described in detail below with reference to the drawings and embodiments.

[0037] This embodiment provides an automatic calculation method for the collision threshold value of an industrial robot based on a servo driver, and its process is as Figure 3 shown, and specifically includes the following steps:

[0038] Step 1. The servo driver establishes communication with the main controller and enters the enabled state to normally control the rotation of the motor;

[0039] Step 2. The servo driver receives the automatic save instruction for the collision detection threshold value sent by the main controller and starts to calculate the collision detection threshold value. Otherwise, it continues to wait for the instruction from the main controller;

[0040] Step 2.1. Collect the motor position information and the motor current information. The collection of the motor position information is usually realized by an encoder installed at the motor end, and the motor current information is collected from the sampling resistor or Hall sensor set by the servo driver in the motor current channel;

[0041] Step 2.2. Calculate the motor speed and motor acceleration based on the acquired motor position information and control period; the motor speed is the first derivative of the position information, and the motor acceleration is the second derivative of the position information;

[0042] Step 2.3. Calculate the motor torque current based on the acquired motor current information and motor position information;

[0043] In this embodiment, an AC permanent magnet synchronous motor is used. First, calculate the α-axis current and β-axis current based on the motor current information, specifically:

[0044] , ;

[0045] where, and respectively represent the α-axis current and β-axis current, both of which are intermediate variables for calculating the torque current; , , are the U, V, and W phase currents of the motor, which are acquired in Step 2.1;

[0046] Then, calculate the equivalent torque current of the motor based on the α-axis current, β-axis current, and motor position information , specifically:

[0047] , ;

[0048] where, represents the electrical angle of the motor, which is collected from the encoder of the motor and calculated from the motor position information; represents the electrical angle offset, usually the prior information given by the motor manufacturer; represents the motor position information, which is acquired in Step 2.1; P represents the number of pole pairs of the motor;

[0049] Step 2.4. Calculate the theoretical torques of the first to sixth joints using robot dynamics based on the motor position information, speed information, acceleration information, and robot physical parameters ~ ; The robot physical parameters include: the lengths of the robot links, the torsional angles of the links, the link offsets, the joint masses, the centroid positions, the inertia tensors, and the robot load; it should be noted that the specific calculation process of the theoretical torque is the prior art in this field, and the present invention will not elaborate;

[0050] Step 2.5. Calculate the feedback torques of the first to sixth joints respectively according to the motor torque current, motor reduction ratio, motor rated torque, and rotation direction ~ , the calculation process of the feedback torque is as follows:

[0051] ;

[0052] Among them, represents the feedback torque, represents the motor torque current, represents the rated torque of the motor, represents the reduction ratio of the motor, represents the rotation direction, The value of is 1 or -1;

[0053] Step 2.6. Calculate the differences between the theoretical torques and the feedback torques of the first to sixth joints respectively ~ , specifically: , represents the difference between the theoretical torque and the feedback torque, represents the theoretical torque, represents the feedback torque;

[0054] Step 2.7. Record the differences between the theoretical torques and the feedback torques of the first to sixth joints in each control cycle within a complete motion cycle, and search for the corresponding maximum values, which are respectively represented as ~ , that is represents the maximum difference between the theoretical torque and the feedback torque of the first joint; Multiply ~ by the same amplification factor for amplification, and the result is represented as ~ , and use ~ as the calculation results of the collision threshold values of the first to sixth joints respectively, and the amplification factor is 1.2;

[0055] Step 3. The servo driver receives the instruction for automatically saving the collision parameter threshold sent by the main controller, saves the calculation results of the collision threshold values of the first to sixth joints to the non-volatile memory, and sends a success flag to the main controller; Saving the calculation results of the collision threshold values to the non-volatile memory can ensure that the collision threshold values are still valid when the power is off or restarted next time;

[0056] Step 4. If the main controller receives the success flag within the preset time window, it completes the automatic calculation of the collision threshold value of the industrial robot, otherwise it gives an alarm prompt, and the preset time window is 2s.

[0057] Thus, this embodiment completes the automatic calculation of the collision detection threshold value of the industrial robot based on the servo driver, so as to improve the working efficiency while realizing the collision detection function of the industrial robot.

[0058] The above are only specific embodiments of the present invention. Any feature disclosed in this specification, unless specifically stated, can be replaced by other equivalent or alternative features with similar purposes; all the disclosed features, or all the steps in any method or process, can be combined in any way except for mutually exclusive features and / or steps.

Claims

1. An automatic calculation method for the collision threshold value of an industrial robot based on a servo drive, characterized in that, It includes the following steps: Step 1. The servo driver establishes communication with the main controller and enters the enabled state; Step 2. The servo driver receives the collision detection threshold automatic saving instruction issued by the main controller and starts calculating the collision detection threshold; Step 2.

1. Collect the motor position information and the motor current information; Step 2.

2. Calculate the motor speed and the motor acceleration according to the collected motor position information and the control period; Step 2.

3. Calculate the motor torque current according to the collected motor current information and the motor position information; Step 2.

4. Based on the motor position information, speed information, acceleration information, and the physical parameters of the robot, the theoretical torques of the first to sixth joints are calculated using robot dynamics ~ ; Step 2.

5. Calculate the feedback torques of the first to sixth joints based on the motor torque current ~ ; Step 2.

6. Calculate the differences between the theoretical torques and the feedback torques of the first joint to the sixth joint respectively ~ ; Step 2.

7. Record the differences between the theoretical torques and the feedback torques of the first to sixth joints in each control cycle during the complete motion cycle, and search for the corresponding maximum values ~ ; Multiply ~ by the same amplification factor for amplification, and the result is expressed as ~ , and use ~ as the calculation results of the collision threshold values for the first to sixth joints respectively; Step 3. The servo driver receives the collision parameter threshold automatic saving end instruction issued by the main controller, saves the calculation results of the collision thresholds of the first joint to the sixth joint to the non-volatile memory, and sends a success flag to the main controller; Step 4. If the main controller receives the success flag within the preset time window, the automatic calculation of the industrial robot collision threshold is completed; otherwise, an alarm prompt is given.

2. The automatic calculation method for the industrial robot collision threshold value based on the servo driver according to claim 1, wherein In Step 2.1, the collection of the motor position information is completed by the encoder installed at the motor end, and the motor current information is collected from the sampling resistor or the Hall sensor set by the servo driver in the motor current channel.

3. The automatic calculation method for the industrial robot collision threshold value based on a servo drive according to claim 1, characterized in that In Step 2.2, the motor speed is the first derivative of the position information, and the motor acceleration is the second derivative of the position information.

4. The automatic calculation method for the industrial robot collision threshold value based on the servo drive according to claim 1, characterized in that, In Step 2.3, first calculate the α-axis current and the β-axis current according to the motor current information, specifically: ， ； Among them, and respectively represent the α-axis current and the β-axis current; , , are the U, V, and W phase currents of the motor; Calculate the equivalent torque current of the motor based on the α-axis current, β-axis current, and motor position information , specifically as follows: ， ； Among them, represents the electrical angle of the motor, represents the electrical angle offset, represents the motor position information, and P represents the number of pole pairs of the motor.

5. The automatic calculation method for the industrial robot collision threshold value based on the servo driver according to claim 1, characterized in that, In Step 2.5, the calculation process of the feedback torque is: ； Among them, represents the feedback torque, represents the motor torque current, represents the rated torque of the motor, represents the motor reduction ratio, represents the rotation direction.

6. The automatic calculation method for the industrial robot collision threshold value based on the servo driver according to claim 1, wherein In Step 2.7, the amplification factor is 1.

2.

7. The automatic calculation method of the industrial robot collision threshold value based on the servo driver according to claim 1, wherein In Step 4, the preset time window is 2s.

Images