Dynamic inertia control method of high-speed heavy-load SCARA robot

Abstract

The invention discloses a dynamic inertia control method for a high-speed heavy-load SCARA robot, and the method comprises the steps: S1, measuring the motion state of each drive joint of the SCARA robot, the SCARA robot comprising a first rotation drive joint, a second rotation drive joint, and a third linear movement drive joint; s2, calculating the mass of the carried object; s3, calculating the equivalent load inertia of a driving joint; s4, dynamic inertia control is implemented; wherein in the step S2, a motor encoder installed at a third linear movement driving joint is used for measuring the acceleration # imgabs0 #, and the mass of a carried object is calculated; calculating the equivalent load inertia of the first rotary driving joint and the second rotary driving joint in real time; and dynamic inertia control voltage signals of the first rotary driving joint and the second rotary driving joint are calculated. By adopting the method, the equivalent load inertia of each driving joint can be automatically measured and calculated, different load control requirements are met, and the control precision is improved.

Description

Technical Field

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

[0002] High-speed and heavy-load SCARA robots belong to a type of nonlinear time-varying coupling system with a large end load. When high-speed and heavy-load SCARA robots carry different large mass loads, the equivalent load inertia of each drive joint changes, which in turn changes the system bandwidth and reduces the servo following accuracy. Therefore, a dynamic inertia control method is urgently needed that can be applied to high-speed and heavy-load SCARA robots to achieve high-precision control under different load conditions. Summary of the invention

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

[0004] To solve the above problems, the present invention provides a dynamic inertia control method for a high-speed heavy-load SCARA robot. To achieve the above objectives, the technical solution adopted by the present invention to solve the technical problems is:

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

[0006] Step S1: measuring the motion state of each driving joint of the SCARA robot, the SCARA robot includes a plurality of driving joints, the driving joints including a first rotation driving joint, a second rotation driving joint, and a third linear motion driving joint;

[0007] Step S2: Calculate the mass of the transported object;

[0008] Step S3: Calculate the equivalent load inertia of the driving joint;

[0009] Step S4: Implement dynamic inertia control;

[0010] In step S2, the motor encoder installed at the third linear motion drive joint is used to measure the acceleration. And calculate the mass of the object being transported;

[0011]

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

[0013]

[0014] Calculating dynamic inertia control voltage signals of the first rotary drive joint and the second rotary drive joint;

[0015]

[0016] As a further improvement of the present invention, in step S2, after the SCARA robot grabs the transported object, a tension sensor installed in the vertical direction of the end of the third linear motion drive joint of the SCARA robot is used to measure the tension value F. At the same time when the tension sensor collects the tension value, the motor encoder installed at the third linear motion drive joint is used to measure the acceleration

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

[0018] As a further improvement of the present invention, in Formula 2 and Formula 3, m0 is the mass of the motor system installed at the second rotational drive joint; l1 is the perpendicular distance between the axis of the first rotational drive joint and the axis of the second rotational drive joint; l2 is the perpendicular distance between the axis of the second rotational drive joint and the moving direction of the third linear motion drive joint.

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

[0020] As a further improvement of the present invention, 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 V is the voltage command signal output by the position loop PID feedback controller of the first rotary drive joint motor; PID2 It is the voltage command signal output by the position loop PID feedback controller of the second rotation drive joint motor.

[0023] As a further improvement of the present invention, the servo drivers of the first rotary drive joint and the second rotary drive joint are set to torque mode, and then the V calculated by formula 5 is cmd1 With V cmd2 It is sent as a control command signal to the servo driver to implement dynamic inertia control.

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

[0025] Measure the rotation angle θ2 and angular velocity of the second rotary drive joint Angular acceleration

[0026] Measure the displacement q3 and speed of the third linear motion drive joint Acceleration

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

[0028] The tension sensor installed at the end of the third linear motion drive joint is used to automatically measure and calculate the equivalent load inertia of each drive joint, thereby adapting to different load 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 large mass loads, the system can respond quickly and adjust the control parameters as needed to ensure that the servo system can accurately follow the preset trajectory and speed, improving the operation accuracy and stability.

[0030] The control method of this application can better adapt to load changes and environmental interference for SCARA robots operating at high speeds and high loads. When the load inertia changes, the system can automatically adjust the control strategy, enhance the robustness of the system, and improve the reliability and durability of the robot.

[0031] In addition, it can also simplify the debugging and maintenance time of the SCARA robot to a certain extent. Because the system can automatically adapt to load changes, the debugger does not need to frequently adjust the control parameters to achieve stable operation of the SCARA robot. BRIEF DESCRIPTION OF THE DRAWINGS

[0032] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0033] Figure 1 It is a structural schematic diagram of an implementation mode of the present invention.

[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 motion drive joint. DETAILED DESCRIPTION

[0035] The present invention is further described in detail below in conjunction with specific embodiments:

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

[0037] Step S1: Measure the motion state of each driving joint of the robot.

[0038] The high-speed and heavy-load SCARA robot 4 includes three driving joints, namely the first rotary driving joint 41, the second rotary driving joint 42 and the third linear motion driving joint 43. The motor encoders installed at each driving joint can measure the rotation angle θ1 and angular velocity θ2 of the first rotary driving joint 41. Angular acceleration The rotation angle θ2 and angular velocity of the second rotary drive joint 42 Angular acceleration The displacement q3 and speed of the third linear motion driving joint 43 Acceleration

[0039] Step S2: Calculate the mass of the transported object.

[0040] After the robot grabs the object to be transported, the tension sensor installed in the vertical direction (Z-axis direction) at the end of the third linear motion drive joint 43 of the robot is used to measure the tension value F. At the same time when the tension sensor collects the tension value, the motor encoder installed at the third linear motion drive joint 43 is used to measure the acceleration Based on this, the mass of the transported object is calculated using the following formula:

[0041]

[0042] Where g = 9.8 m / s 2 Represents the acceleration due to gravity.

[0043] Step S3: Calculate the equivalent load inertia of the driving 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 using the following formula:

[0045]

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

[0047] Step S4: Implement dynamic inertia control.

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

[0049]

[0050] Wherein, [τ] 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. Based on this, the control voltage command signal is calculated using the following formula.

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

[0052] Where V PID1 represents the voltage command signal output by the position loop PID feedback controller 1 of the motor 3 of the first rotary drive joint 41; V PID2 Represents the voltage command signal output by the position loop PID feedback controller 1 of the motor 3 of the second rotary drive joint 42 .

[0053] Set the servo driver 2 of the first rotary drive joint 41 and the second rotary drive joint 42 to torque mode, and then use the V calculated by the above formula cmd1 With V cmd2 It is sent as a control command signal to the driver to implement dynamic inertia control.

[0054] In other embodiments of the present application, an indicator light is connected between the PID feedback controller 1 and the servo driver 2, and an indicator light is also connected upstream of the PID feedback controller 1. The indicator light upstream of the PID feedback controller 1 is also electrically connected in parallel with the downstream of the motor 3 through a wire. The indicator light 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 embodiments are only for illustrating the technical concept and features of the present invention, and their purpose is to enable people familiar with this technology to understand the content of the present invention and implement it. They cannot be used to limit the protection scope of the present invention. Any equivalent changes or modifications made according to the spirit of the present invention should be included in the protection scope of the present invention.

Claims

1. A dynamic inertia control method for a high-speed and heavy-load SCARA robot, characterized in that: include: Step S1: measuring the motion state of each driving joint of the SCARA robot, wherein the SCARA robot includes a plurality of driving joints, and the driving joints include a first rotation driving joint, a second rotation driving joint, and a third linear motion driving joint; Step S2: Calculate the mass of the transported object; Step S3: Calculate the equivalent load inertia of the driving joint; Step S4: Implement dynamic inertia control; In step S2, the motor encoder installed at the third linear motion drive joint is used to measure the acceleration. And calculate the mass of the object being transported; Calculate the equivalent load inertia of the first rotary drive joint and the second rotary drive joint in real time; Calculating dynamic inertia control voltage signals of the first rotary drive joint and the second rotary drive joint; 2. The dynamic inertia control method of a high-speed and heavy-load SCARA robot according to claim 1, characterized in that: In step S2, after the SCARA robot grabs the object being transported, the tension value F is measured by the tension sensor installed in the vertical direction of the end of the third linear motion drive joint of the SCARA robot. At the same time when the tension sensor collects the tension value, the acceleration is measured by the motor encoder installed at the third linear motion drive joint. In the formula 1, g is the acceleration due to gravity, g = 9.8 m / s 2 .

3. The dynamic inertia control method of a high-speed and heavy-load SCARA robot according to claim 1, characterized in that: In Formula 2 and Formula 3, m0 is the mass of the motor system installed at the second rotary drive joint; l1 is the perpendicular distance between the axis of the first rotary drive joint and the axis of the second rotary drive joint; l2 is the perpendicular distance between the axis of the second rotary drive joint and the moving direction of the third linear motion drive joint.

4. The dynamic inertia control method of a high-speed and heavy-load SCARA robot according to claim 1, characterized in that: In Formula 4, [τ] is the absolute value of the peak torque of the driving joint motor, and [V] is the absolute value of the analog voltage corresponding to the peak torque of the driving joint motor.

5. The dynamic inertia control method of a high-speed and heavy-load SCARA robot according to claim 1, characterized in that: In the step S4, a control voltage command signal is calculated; V cmd1 =V1+V PID1 ,V cmd2 =V2+V PID2 (formula 5) In Formula 5, V PID1 V is the voltage command signal output by the position loop PID feedback controller of the first rotary drive joint motor; PID2 It is the voltage command signal output by the position loop PID feedback controller of the second rotation drive joint motor.

6. The dynamic inertia control method of a high-speed and heavy-load SCARA robot according to claim 5, characterized in that: Set the servo drivers of the first rotary drive joint and the second rotary drive joint to torque mode, and then use the V calculated by Formula 5 cmd1 With V cmd2 It is sent as a control command signal to the servo driver to implement dynamic inertia control.

7. The dynamic inertia control method of a high-speed and heavy-load SCARA robot according to claim 1, characterized in that: In step S1, the motor encoder installed at each driving joint of the SCARA robot is used to measure the rotation angle θ1 and angular velocity of the first rotating driving joint. Angular acceleration Measure the rotation angle θ2 and angular velocity of the second rotary drive joint Angular acceleration Measure the displacement q3 and speed of the third linear motion drive joint Acceleration

Images