Dynamic inertia control method for high-speed heavy-duty scara robot

Abstract

The application discloses a dynamic inertia control method of a high-speed heavy-load SCARA robot, which comprises the following steps: S1, measuring the motion state of each driving joint of the SCARA robot, wherein the SCARA robot comprises a first rotary driving joint, a second rotary driving joint and a third linear moving driving joint; S2, calculating the mass of a carried object; S3, calculating the equivalent load inertia of the driving joint; and S4, implementing dynamic inertia control; wherein the mass of the carried object is calculated by measuring the acceleration by using a motor encoder installed at the third linear moving driving joint in the step S2; the equivalent load inertia of the first rotary driving joint and the second rotary driving joint is calculated in real time; and the dynamic inertia control voltage signal of the first rotary driving joint and the second rotary driving joint is calculated. The application can automatically measure and calculate the equivalent load inertia of each driving joint, adapt to different load control requirements and improve the control precision.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of robotics and automation, in particular to a dynamic inertia control method of a high-speed heavy-load SCARA robot. BACKGROUND

[0002] The high-speed heavy-load SCARA robot belongs to a class of nonlinear time-varying coupled systems, and its end load is large. When the high-speed heavy-load SCARA robot carries different heavy loads, the equivalent load inertia of each drive joint changes, thereby changing the system bandwidth and reducing the servo following accuracy. Therefore, a dynamic inertia control method is urgently needed, which can be applied to the high-speed heavy-load SCARA robot to achieve high-precision control under different load conditions. SUMMARY

[0003] The problem to be solved by the present application is to provide a dynamic inertia control method of a high-speed heavy-load SCARA robot, which can automatically measure and calculate the equivalent load inertia of each drive joint, adapt to different load control requirements, and improve the control accuracy.

[0004] To solve the above problems, the present application provides a dynamic inertia control method of a high-speed heavy-load SCARA robot. To achieve the above purpose, the technical scheme adopted by the present application to solve its technical problems is:

[0005] A dynamic inertia control method of a high-speed heavy-load SCARA robot, comprising:

[0006] Step S1: measuring the motion state of each drive joint of the SCARA robot, the SCARA robot comprising a plurality of drive joints, the drive joints comprising a first rotary drive joint, a second rotary drive joint, and a third linear motion drive joint;

[0007] Step S2: calculating the mass of the carried object;

[0008] Step S3: calculating the equivalent load inertia of the drive joint;

[0009] Step S4: implementing dynamic inertia control;

[0010] In step S2, the acceleration is measured by a motor encoder installed at the third linear motion drive joint. and the mass of the carried object is calculated;

[0011]

[0012] The equivalent load inertia of the first rotary drive joint and the second rotary drive joint is calculated in real time;

[0013]

[0014] calculating a first rotary drive joint and a second rotary drive joint dynamic inertia control voltage signal;

[0015]

[0016] As a further improvement of the present application, in step S2, after the SCARA robot picks up the object to be carried, the tension value F is measured by using the tension sensor installed in the vertical direction of the end of the third linear movement drive joint of the SCARA robot, and at the same time when the tension sensor collects the tension value, the acceleration is measured by using the motor encoder installed at the third linear movement drive joint

[0017] In formula 1, g is the acceleration of gravity, g = 9.8 m / s 2 .

[0018] As a further improvement of the present application, in formula 2 and formula 3, m0 is the mass of the motor system installed at the second rotary drive joint; l1 is the vertical distance between the axis of the first rotary drive joint and the axis of the second rotary drive joint; l2 is the vertical distance between the axis of the second rotary drive joint and the movement direction of the third linear movement drive joint.

[0019] As a further improvement of the present application, in formula 4, [τ] is the peak torque absolute value of the drive joint motor, and [V] is the analog voltage absolute value corresponding to the peak torque of the drive joint motor.

[0020] As a further improvement of the present application, in step S4, the control voltage command signal is calculated

[0021] V cmd1 = V1 + V PID1 , V cmd2 = V2 + V PID2 (Formula 5)

[0022] In formula 5, V PID1 is the voltage command signal output by the position loop PID feedback controller of the first rotary drive joint motor; V PID2 is the voltage command signal output by the position loop PID feedback controller of the second rotary drive joint motor.

[0023] As a further improvement of the present application, the servo drive of the first rotary drive joint and the second rotary drive joint is set to torque mode, and then V cmd1 and V cmd2 are sent to the servo drive as control command signals to implement dynamic inertia control.

[0024] As a further improvement of the application, in step S1, the motor encoder installed at each driving joint of the SCARA robot is used to measure the rotation angle θ1, angular velocity ω1 of the first rotation driving joint angular acceleration

[0025] The motor encoder installed at each driving joint of the SCARA robot is used to measure the rotation angle θ2, angular velocity ω2 of the second rotation driving joint angular acceleration

[0026] The motor encoder installed at each driving joint of the SCARA robot is used to measure the displacement q3, velocity v3 of the third linear movement driving joint acceleration

[0027] The dynamic inertia control method of the high-speed heavy-load SCARA robot of the present application has the following beneficial effects:

[0028] The equivalent load inertia of each driving joint is automatically measured and calculated by the tension sensor installed at the end of the third linear movement driving joint, thereby adapting to different control requirements and improving control accuracy.

[0029] The servo following accuracy is improved because the dynamic inertia control method can adjust the equivalent load inertia of each driving joint in real time, thereby maintaining the stability of the system bandwidth. Therefore, when the robot carries different heavy loads, the system can quickly respond and adjust the control parameters as needed to ensure that the servo system can accurately follow the preset trajectory and speed, thereby improving the work accuracy and stability.

[0030] The control method of the present application can better adapt to load changes and environmental disturbances for SCARA robots working at high speed and high load. When the load inertia changes, the system can automatically adjust the control strategy. The robustness of the system is enhanced, and the reliability and durability of the robot are improved.

[0031] In addition, the debugging and maintenance time of the SCARA robot can also be simplified to a certain extent. Because the system can automatically adapt to load changes, the debugging personnel do not need to frequently adjust the control parameters to realize stable operation of the SCARA robot. BRIEF DESCRIPTION OF DRAWINGS

[0032] In order to more clearly illustrate the technical solutions in the embodiments of the present application or the prior art, the following will briefly introduce the drawings needed to be used in the embodiment or prior art description. Obviously, the drawings in the following description only constitute some embodiments of the present application, and those skilled in the art can also obtain other drawings according to these drawings without creative labor.

[0033] Figure 1 is a structural schematic diagram of an embodiment of the present application.

[0034] 1 - PID feedback controller; 2 - servo driver; 3 - motor; 4 - SCARA robot; 41 - first rotary drive joint; 42 - second rotary drive joint; 43 - third linear movement drive joint. DETAILED DESCRIPTION

[0035] The present application will be further described in conjunction with specific embodiments:

[0036] In order to achieve the purpose of the present application, a dynamic inertia control method of a high-speed heavy-load SCARA robot of the present application comprises the following steps:

[0037] Step S1: measuring the motion state of each drive joint of the robot.

[0038] The high-speed heavy-load SCARA robot 4 comprises three drive joints, i.e. the first rotary drive joint 41, the second rotary drive joint 42 and the third linear movement drive joint 43. The rotation angle θ1, angular velocity ω1 and angular acceleration α1 of the first rotary drive joint 41 can be measured by using the motor encoder installed at the first rotary drive joint 41. Angular acceleration The rotation angle θ2, angular velocity ω2 and angular acceleration α2 of the second rotary drive joint 42 can be measured by using the motor encoder installed at the second rotary drive joint 42. Angular acceleration The displacement q3, velocity v3 and acceleration a3 of the third linear movement drive joint 43 can be measured by using the motor encoder installed at the third linear movement drive joint 43. Acceleration

[0039] Step S2: calculating the mass of the carried object.

[0040] After the robot grasps the carried object, the tension value F is measured by using the tension sensor installed at the end of the third linear movement drive joint 43 in the vertical direction (Z-axis direction). At the same time when the tension sensor collects the tension value, the acceleration a3 of the third linear movement drive joint 43 is measured by using the motor encoder installed at the third linear movement drive joint 43. Accordingly, the mass of the carried object is calculated by using the following formula

[0041]

[0042] In the formula, g = 9.8 m / s 2 represents the gravitational acceleration.

[0043] Step S3: calculating the equivalent load inertia of the drive joint.

[0044] The equivalent load inertia of the first rotary drive joint 41 and the second rotary drive joint 42 is calculated in real time by using the following formula

[0045]

[0046] In the formula, m0 represents the system mass of the motor 3 installed at the second rotary driving joint 42, which can be obtained by consulting the motor 3 selection manual; l1 represents the vertical distance between the axis of the first rotary driving joint 41 and the axis of the second rotary driving joint 42; and l2 represents the vertical distance between the axis of the second rotary driving joint 42 and the moving direction of the third linear moving driving joint 43.

[0047] Step S4: Implementing dynamic inertia control.

[0048] The dynamic inertia control voltage signal of the first rotary driving joint 41 and the second rotary driving joint 42 is calculated by using the following formula

[0049]

[0050] In the formula, [τ] represents the absolute value of the peak torque of the driving joint motor 3, and [V] represents the absolute value of the analog voltage corresponding to the peak torque of the driving joint motor 3. According to this, the control voltage command signal is calculated by using the following formula.

[0051] V cmd1 = V1+ V PID1 , V cmd2 = V2+ V PID2 (Formula 5)

[0052] In the formula, V PID1 represents the voltage command signal output by the position loop PID feedback controller 1 of the motor 3 of the first rotary driving joint 41; and V PID2 represents the voltage command signal output by the position loop PID feedback controller 1 of the motor 3 of the second rotary driving joint 42.

[0053] The servo driver 2 of the first rotary driving joint 41 and the second rotary driving joint 42 is set to the torque mode, and then V cmd1 and V cmd2 calculated by the above formula are sent to the driver as the control command signal to implement the dynamic inertia control.

[0054] In some other embodiments of the present application, an indicator lamp is connected between the PID feedback controller 1 and the servo driver 2, and an indicator lamp is also connected upstream of the PID feedback controller 1. The indicator lamp upstream of the PID feedback controller 1 is also electrically connected in parallel with the downstream motor 3 through a wire. The indicator lamp between the PID feedback controller 1 and the servo driver 2 is directly connected to the output end of the dynamic inertia control method.

[0055] The above examples are only for illustrating the technical concept and characteristics of the present application, and the purpose is to enable those skilled in the art to understand the content of the present application and implement it, and cannot limit the protection scope of the present application. Any equivalent changes or modifications made according to the spirit and essence of the present application shall be covered within the protection scope of the present application.

Claims

1. A dynamic inertia control method for a high-speed, heavy-duty SCARA robot, characterized in that, include: Step S1: Measure the motion state of each drive joint of the SCARA robot. The SCARA robot includes several drive joints, including a first rotary drive joint, a second rotary drive joint, and a third linear motion drive joint. Step S2: Calculate the mass of the object being moved; Step S3: Calculate the equivalent load inertia of the drive joint; Step S4: Implement dynamic inertia control; In step S2, the acceleration is measured using a motor encoder installed at the third linear motion drive joint. And calculate the mass of the object being transported; (Official 1); The equivalent load inertia of the first rotary drive joint and the second rotary drive joint are calculated in real time. (Official 2); (Official 3); Calculate the dynamic inertia control voltage signals for the first and second rotary drive joints; , (Official 4); In step S1, the rotation angle of the first rotary drive joint is measured using motor encoders installed at each drive joint of the SCARA robot. angular velocity angular acceleration ; Measure the rotation angle of the second rotary drive joint angular velocity angular acceleration ; Measuring the displacement of the third linear motion drive joint ,speed acceleration ; In step S2, after the SCARA robot grasps the object being transported, it measures the tension value using a tension sensor installed in the vertical direction at the end of the third linear motion drive joint of the SCARA robot. At the same moment that the tension sensor collects the tension value, the acceleration is measured using the motor encoder installed at the third linear motion drive joint. ; In Formula 1, g is the acceleration due to gravity. ; In formulas 2 and 3, The mass of the motor system installed at the second rotary drive joint; The perpendicular distance between the axis of the first rotational drive joint and the axis of the second rotational drive joint; The perpendicular distance between the axis of the second rotational drive joint and the direction of movement of the third linear drive joint; In formula 4, This refers to the absolute value of the peak torque driving the joint motor. This is the absolute value of the analog voltage corresponding to the peak torque of the drive joint motor.

2. The dynamic inertia control method for a high-speed, heavy-duty SCARA robot according to claim 1, characterized in that: In step S4, the control voltage command signal is calculated; , (Official 5) In formula 5, The voltage command signal output by the position loop PID feedback controller of the first rotary drive joint motor; The voltage command signal is output by the position loop PID feedback controller of the second rotary drive joint motor.

3. The dynamic inertia control method for a high-speed, heavy-duty SCARA robot according to claim 2, characterized in that: Set the servo drivers of the first and second rotary drive joints to torque mode, and then calculate the result using formula 5. and The control command signal is sent to the servo driver to implement dynamic inertia control.

Images