A robot motion control method, control system, medium, and product

Abstract

This application discloses a robot motion control method, control system, medium, and product, relating to the field of robot motion control. Implementing this application, the control system constructs a digital twin model of the robot, determines the theoretical motion response data of the robot executing motor drive commands, and simultaneously collects the actual motion state data of the robot executing motor drive commands through equipment. The control system separates the actual motion state data into low-frequency motion components and high-frequency vibration components: the low-frequency motion component is used for trajectory correction, generating position compensation commands based on pose deviation; the high-frequency vibration component is used for vibration suppression, determining the stiffness coefficient through energy analysis and generating impedance adjustment commands. This dual feedback control mechanism ensures both the accuracy of the motion trajectory and the suppression of high-frequency vibrations, improving the positioning accuracy and stability of the robot during high-speed movement.

Description

Technical Field

[0001] This application relates to the field of robot motion control, and in particular to a robot motion control method, control system, medium, and product. Background Technology

[0002] With the continuous improvement of industrial automation, the application of robots is becoming increasingly widespread. In processes such as precision assembly and polishing, which have stringent requirements for process accuracy, robots need to have high-precision and high-stability motion control capabilities. Especially under high-speed motion conditions, the accuracy and stability of robot motion control have a significant impact on processing quality.

[0003] Currently, robot motion control methods mainly employ PID-based position servo control. This involves setting proportional, integral, and derivative parameters, calculating the control input based on the deviation between the actual and target positions, and thus achieving closed-loop control of the robot's joint motors. In practical applications, the controller sends drive commands to each joint motor according to a predetermined trajectory and uses encoder feedback for position correction to ensure the robot moves along the predetermined path.

[0004] However, during high-speed movement, due to the inherent characteristics of the mechanical structure, the joint drive system of a robot will vibrate, causing the end effector to jitter. This jitter reduces the robot's positioning accuracy and affects the quality of workpiece processing. Summary of the Invention

[0005] This application provides a robot motion control method, control system, medium, and product for improving the positioning accuracy and stability of robots during high-speed movement.

[0006] Firstly, this application provides a robot motion control method applied to a control system. The method includes: sending motor drive commands to a robot and inputting the motor drive commands into a robot digital twin model to obtain theoretical motion response data of the robot. The motor drive commands are used to control the movement of each joint of the robot along a predetermined trajectory. The robot digital twin model refers to a virtual model pre-constructed based on the robot's dynamic and kinematic characteristics; acquiring actual motion state data of the robot executing the motor drive commands, including actual joint angles collected by joint encoders and actual vibration accelerations collected by joint inertial sensors; and processing the actual motion state data to separate low-frequency components. The system consists of low-frequency motion components and high-frequency vibration components. The low-frequency motion component represents the robot's motion trajectory, while the high-frequency vibration component represents the robot's jitter noise. Based on the low-frequency motion component, the robot's actual motion pose is determined. The actual motion pose is compared with the theoretical motion pose to obtain the pose deviation. The theoretical motion pose is calculated from the theoretical motion response data. Energy analysis is performed on the high-frequency vibration component to obtain the vibration energy density. Combined with a preset vibration energy-stiffness mapping relationship, the stiffness coefficient used for vibration suppression is determined. Based on the pose deviation, a position compensation command for trajectory correction is generated, and based on the stiffness coefficient, an impedance adjustment command for vibration suppression is generated. The position compensation command and impedance adjustment command are then sent to the robot.

[0007] By adopting the above technical solution, the control system constructs a digital twin model of the robot, determines the theoretical motion response data of the robot executing motor drive commands, and simultaneously collects the actual motion state data of the robot executing motor drive commands through equipment. The control system separates the actual motion state data into low-frequency motion components and high-frequency vibration components: the low-frequency motion component is used for trajectory correction, generating position compensation commands based on pose deviation; the high-frequency vibration component is used for vibration suppression, determining the stiffness coefficient through energy analysis and generating impedance adjustment commands. This dual feedback control mechanism ensures both the accuracy of the motion trajectory and the suppression of high-frequency vibrations, improving the robot's positioning accuracy and stability during high-speed movement. Through closed-loop control of theoretical motion response data and actual motion state data, precise control of the robot's motion is achieved, effectively solving the problem of insufficient vibration suppression capability of traditional PID control under high-speed conditions.

[0008] In conjunction with some embodiments of the first aspect, in some embodiments, the actual motion state data is processed to separate low-frequency motion components and high-frequency vibration components. The low-frequency motion components represent the robot's motion trajectory, and the high-frequency vibration components represent the robot's jitter noise. Specifically, this includes: inputting the actual joint angles into a preset low-pass filter and outputting the robot's smooth angular trajectory as the low-frequency motion component; and inputting the actual vibration acceleration into a preset high-pass filter and outputting the robot's jitter acceleration data as the high-frequency vibration component.

[0009] By adopting the above technical solution, the control system performs frequency division processing on the actual motion state data through preset low-pass and preset high-pass filters, effectively separating the robot's pure motion trajectory from pure jitter noise. The preset low-pass filter removes high-frequency interference while retaining the low-frequency signal reflecting the actual motion trajectory; the preset high-pass filter extracts the high-frequency signal reflecting the structural vibration characteristics. This signal separation processing method avoids interference from vibration signals on trajectory control, while ensuring the targetedness and effectiveness of vibration suppression. Compared with directly using raw sensor data for control, it improves the robustness and accuracy of the control.

[0010] In conjunction with some embodiments of the first aspect, in some embodiments, the actual motion pose of the robot is determined based on the low-frequency motion components, specifically including: acquiring the robot's link geometry parameters and DH parameters to construct the robot's forward kinematics model; determining the smoothed angle data of each joint based on the low-frequency motion components, substituting them into the forward kinematics model, and calculating the position coordinates and attitude angles of the robot's end effector relative to the base coordinate system through homogeneous transformation matrix multiplication; and using the position coordinates and attitude angles as the actual motion pose.

[0011] By adopting the above technical solution, the control system constructs a forward kinematics model based on the robot's link geometry parameters and DH parameters, and calculates the position coordinates and attitude angles of the robot's end effector through homogeneous transformation matrix multiplication. This kinematic analysis method based on robot structural parameters can accurately calculate the mapping relationship between each joint angle and the end effector pose. The control system substitutes the smoothed angle data after low-pass filtering into the forward kinematics model to accurately obtain the robot's actual motion pose, avoiding interference from high-frequency vibrations in pose calculation, and providing reliable feedback data for subsequent trajectory correction, thus ensuring the accuracy of control.

[0012] In conjunction with some embodiments of the first aspect, in some embodiments, energy analysis is performed on the high-frequency vibration components to obtain the vibration energy density. Combined with a preset vibration energy-stiffness mapping relationship, the stiffness coefficient used for vibration suppression is determined. Specifically, this includes: performing power spectral density analysis on the target high-frequency vibration components corresponding to the target joint to obtain the vibration energy distribution curve of the target joint, where the target joint is any joint; calculating the integral value of the vibration energy distribution curve to obtain the total vibration energy of the target joint; and mapping the total vibration energy to the target stiffness coefficient of the target joint according to the preset vibration energy-stiffness mapping relationship.

[0013] By employing the above technical solution, the control system performs power spectral density analysis on the high-frequency vibration components, accurately obtaining the vibration energy distribution curves of each joint of the robot. By calculating the integral value of the vibration energy distribution curves, the vibration intensity of each joint can be accurately quantified. The control system uses a preset vibration energy-stiffness mapping relationship, which allows for adaptive adjustment of the stiffness coefficient based on the vibration intensity, achieving precise vibration suppression. This energy analysis-based stiffness adjustment method overcomes the poor adaptability of traditional fixed stiffness control schemes, dynamically optimizing control parameters according to the actual vibration state, improving vibration suppression effectiveness, and is particularly suitable for precision control under high-speed motion conditions.

[0014] In conjunction with some embodiments of the first aspect, in some embodiments, a position compensation command for trajectory correction is generated based on the pose deviation, specifically including: substituting the pose deviation into the position compensation calculation formula to obtain the position compensation amount, so as to generate the position compensation command; calculating the impedance force based on the stiffness coefficient, so as to generate the impedance adjustment command; the position compensation calculation formula is: ΔP=Kp×(Pd-Pa)+Kd×d(Pd-Pa) / dt; where ΔP is the position compensation amount, Pd is the theoretical motion pose, Pa is the actual motion pose, Kp is the position proportional gain, and Kd is the position differential gain.

[0015] By adopting the above technical solution, the control system employs a position compensation calculation formula that includes proportional and derivative terms. Through the synergistic effect of the position proportional gain Kp and the position derivative gain Kd, both position tracking accuracy and dynamic response characteristics are ensured. The proportional term provides a basic correction based on the position deviation, while the derivative term provides damping by introducing velocity information, effectively suppressing overshoot and oscillation. This improved PD control strategy has better dynamic performance than simple proportional control, improving tracking accuracy while ensuring control stability, and is suitable for industrial applications requiring rapid response and high-precision positioning.

[0016] In conjunction with some embodiments of the first aspect, in some embodiments, an impedance adjustment command for vibration suppression is generated based on the stiffness coefficient. Specifically, this includes: calculating the corresponding damping coefficient using the critical damping formula based on the stiffness coefficient; determining the joint angle deviation based on the pose deviation; calculating the difference between the theoretical joint angular velocity and the actual joint angular velocity to obtain the joint angular velocity deviation, wherein the theoretical joint angular velocity is determined based on theoretical motion response data and the actual joint angular velocity is determined based on actual motion state data; substituting the stiffness coefficient, damping coefficient, joint angle deviation, and joint angular velocity deviation into a second-order impedance model to calculate the joint torque to be compensated; and using the joint torque as the impedance adjustment command. The second-order impedance model is: F = K × E + B × V; where F is the joint torque, K is the stiffness coefficient, E is the joint angle deviation, B is the damping coefficient, and V is the joint angular velocity deviation.

[0017] By adopting the above technical solution, the control system achieves vibration suppression based on a second-order impedance model. This model includes two key parameters: a stiffness coefficient K to ensure joint position accuracy and a damping coefficient B to suppress vibration. This control scheme, combining stiffness and damping, can accurately control robot joint motion, enabling rapid vibration suppression during movement. Compared to traditional methods using only stiffness control, this approach improves both the dynamic performance and stability of joint motion control, effectively suppressing joint vibration under various working conditions.

[0018] In conjunction with some embodiments of the first aspect, in some embodiments, after sending the position compensation command and impedance adjustment command to the robot, the method further includes: acquiring corrected motion state data after the robot has executed the position compensation command and impedance adjustment command; calculating the residual deviation between the corrected motion state data and the theoretical motion response data; when the residual deviation is greater than a preset model update threshold, using the corrected motion state data to correct the dynamic parameters of the robot's digital twin model, wherein the dynamic parameters include at least the joint friction coefficient and the link mass distribution parameter.

[0019] By adopting the above technical solution, the control system introduces an online update mechanism for the robot's digital twin model. This mechanism dynamically optimizes the model parameters by comparing and correcting deviations between the motion state data and the theoretical motion response data. This adaptive learning mechanism enables the control system to adapt to dynamic changes in robot characteristics, such as changes in tool load and joint wear, maintaining long-term control accuracy. Simultaneously, the continuously optimized robot digital twin model provides more accurate predictive support for subsequent motion planning and control.

[0020] In a second aspect, embodiments of this application provide a control system comprising: one or more processors and a memory; the memory is coupled to the one or more processors and is used to store computer program code, the computer program code including computer instructions, wherein the one or more processors invoke the computer instructions to cause the control system to perform the method described in the first aspect and any possible implementation thereof.

[0021] Thirdly, embodiments of this application provide a computer program product containing instructions that, when the computer program product is run on a control system, cause the control system to perform the method described in the first aspect and any possible implementation thereof.

[0022] Fourthly, embodiments of this application provide a computer-readable storage medium including instructions that, when executed on a control system, cause the control system to perform the method described in the first aspect and any possible implementation thereof.

[0023] Understandably, the control system provided in the second aspect, the computer program product provided in the third aspect, and the computer storage medium provided in the fourth aspect are all used to execute the methods provided in the embodiments of this application. Therefore, the beneficial effects they can achieve can be referred to the beneficial effects in the corresponding methods, and will not be repeated here.

[0024] One or more technical solutions provided in the embodiments of this application have at least the following technical effects or advantages:

[0025] 1. By adopting the above technical solution, the control system constructs a digital twin model of the robot to determine the theoretical motion response data of the robot executing motor drive commands. Simultaneously, it collects the actual motion state data of the robot executing these commands through equipment. The control system separates the actual motion state data into low-frequency motion components and high-frequency vibration components: the low-frequency motion component is used for trajectory correction, generating position compensation commands based on pose deviation; the high-frequency vibration component is used for vibration suppression, determining the stiffness coefficient through energy analysis and generating impedance adjustment commands. This dual feedback control mechanism ensures both the accuracy of the motion trajectory and the suppression of high-frequency vibrations, improving the robot's positioning accuracy and stability during high-speed movement. Through closed-loop control of theoretical motion response data and actual motion state data, precise control of the robot's motion is achieved, effectively solving the problem of insufficient vibration suppression capability of traditional PID control under high-speed conditions.

[0026] 2. By adopting the above technical solution, the control system performs frequency division processing on the actual motion state data through preset low-pass and preset high-pass filters, effectively separating the robot's pure motion trajectory from pure jitter noise. The preset low-pass filter removes high-frequency interference while retaining the low-frequency signal reflecting the actual motion trajectory; the preset high-pass filter extracts the high-frequency signal reflecting the structural vibration characteristics. This signal separation processing method avoids interference from vibration signals on trajectory control, while ensuring the targetedness and effectiveness of vibration suppression. Compared with directly using raw sensor data for control, it improves the robustness and accuracy of control.

[0027] 3. By adopting the above technical solution, the control system performs power spectral density analysis on the high-frequency vibration components, which can accurately obtain the vibration energy distribution curves of each joint of the robot. By calculating the integral value of the vibration energy distribution curves, the vibration intensity of each joint can be accurately quantified. The control system uses a preset vibration energy-stiffness mapping relationship, which can adaptively adjust the stiffness coefficient according to the vibration intensity to achieve precise vibration suppression. This stiffness adjustment method based on energy analysis overcomes the shortcomings of poor adaptability of traditional fixed stiffness control schemes, and can dynamically optimize control parameters according to the actual vibration state, improving the vibration suppression effect. It is particularly suitable for precision control under high-speed motion conditions. Attached Figure Description

[0028] Figure 1 This is a flowchart illustrating a robot motion control method in an embodiment of this application;

[0029] Figure 2 This is another flowchart illustrating the robot motion control method in this application embodiment;

[0030] Figure 3 This is a schematic diagram of the physical device structure of a control system in an embodiment of this application. Detailed Implementation

[0031] The terminology used in the following embodiments of this application is for the purpose of describing particular embodiments only and is not intended to be limiting of this application. As used in the specification of this application, the singular expressions “a,” “an,” “the,” “the,” and “this” are intended to include the plural expressions as well, unless the context clearly indicates otherwise. It should also be understood that the term “and / or” as used in this application refers to any or all possible combinations including one or more of the listed items.

[0032] Hereinafter, the terms "first" and "second" are used for descriptive purposes only and should not be construed as implying or suggesting relative importance or implicitly indicating the number of indicated technical features. Thus, a feature defined as "first" or "second" may explicitly or implicitly include one or more of that feature, and in the description of the embodiments of this application, unless otherwise stated, "multiple" means two or more.

[0033] The following describes the process of the method provided in this implementation. Please refer to [link / reference]. Figure 1 This is a flowchart illustrating a robot motion control method in an embodiment of this application.

[0034] S101. Send motor drive commands to the robot and input the motor drive commands into the robot's digital twin model to obtain the robot's theoretical motion response data. The motor drive commands are used to control the robot's joints to move along a predetermined trajectory. The robot's digital twin model is a virtual model that is pre-built based on the robot's dynamic and kinematic characteristics.

[0035] Among them, motor drive commands represent the control signals used by the control system to control the movement of the robot's joint motors, including speed commands, position commands, and torque commands; the predetermined trajectory refers to the pre-planned spatial path that the robot's end effector needs to follow, which is usually composed of a series of discrete position points and attitude angles; the robot digital twin model refers to a numerical model built in a virtual environment that has the same geometric features, kinematic characteristics, and dynamic characteristics as the physical robot; theoretical motion response data represents the robot's motion state predicted by the robot digital twin model after receiving motor drive commands, including time-domain data such as joint angles, angular velocities, and accelerations; dynamic characteristics refer to the physical characteristics that describe the relationship between robot motion and force, including parameters such as mass, inertia, and friction; and kinematic characteristics refer to the geometric characteristics that describe the mapping relationship between the robot's joint angles and end effector pose.

[0036] Specifically, the control system generates a sequence of motor drive commands for each joint based on a predetermined trajectory, including the target angle, target angular velocity, and target torque at each sampling moment. Simultaneously, the control system inputs the same sequence of motor drive commands for each joint into a pre-established digital twin model of the robot for simulation calculations. The digital twin model establishes complete dynamic equations based on the robot's structural parameters, mass distribution, and friction characteristics. Through numerical integration, it can predict the robot's ideal motion response data under motor drive commands.

[0037] The process of constructing a robot digital twin model is as follows:

[0038] (1) Establish a geometric model: Based on the robot's CAD data, construct a three-dimensional geometric model containing each link and joint, and determine structural parameters such as joint type and link size.

[0039] (2) Establishing a kinematic model: Determine the DH parameters based on the robot's linkage structure, establish the coordinate system of each joint, derive the homogeneous transformation matrix, establish the mapping relationship from joint space to Cartesian space, and construct the Jacobian matrix for velocity and force mapping calculation.

[0040] (3) Establish a dynamic model: calculate the mass, center of gravity position, and inertia tensor of each link, establish the Lagrange equation, derive the dynamic equation of the robot, identify dynamic parameters such as joint friction coefficient and motor characteristic parameters, and consider dynamic effects such as gravity and Coriolis force.

[0041] The specific form of the motor drive command is as follows:

[0042] (1) Position control command: such as "J1=30°, J2=-45°, J3=60°", specifying the target angle of each joint; (2) Velocity control command: such as "V1=10rad / s, V2=-5rad / s, V3=8rad / s", specifying the target angular velocity of each joint; (3) Torque control command: such as "T1=50Nm, T2=-30Nm, T3=40Nm", specifying the target torque of each joint; (4) Mixed control command: simultaneously includes position, velocity and torque parameters... others are not listed here.

[0043] After inputting the motor drive commands into the robot's digital twin model, the motor drive commands are first converted into state variables, then substituted into the dynamic equations for solution, and numerical integration is performed to output theoretical motion response data. The theoretical motion response data includes joint angle time series, joint angular velocity time series, joint angular acceleration time series, end-effector pose time series, end-effector velocity time series, etc.

[0044] S102. Obtain the actual motion state data of the robot executing motor drive commands. The actual motion state data includes the actual joint angles collected by the joint encoder and the actual vibration acceleration collected by the joint inertial sensor.

[0045] Among them, actual motion state data represents the measured motion parameters of the robot during the execution of motor drive commands; joint encoders are sensors installed at each joint of the robot to measure joint rotation angles, usually photoelectric encoders or magnetic encoders; actual joint angles represent the actual rotation angles of each joint relative to the zero position; joint inertial sensors are accelerometers or gyroscopes installed at each joint of the robot to measure joint vibration acceleration; actual vibration acceleration represents the acceleration components in each direction generated by the robot joints during movement, reflecting the structural vibration characteristics.

[0046] Specifically, the control system continuously reads the angle data from the encoders of each joint via a real-time communication interface, with a sampling frequency typically above 1 kHz to ensure the temporal resolution of the data. Simultaneously, the control system continuously reads the vibration acceleration data from the inertial sensors of each joint via the same real-time communication interface. After preliminary signal conditioning and filtering, this data forms the actual motion state data, which is used for subsequent motion analysis and control calculations.

[0047] S103. Process the actual motion state data to separate the low-frequency motion component and the high-frequency vibration component. The low-frequency motion component represents the robot's motion trajectory, and the high-frequency vibration component represents the robot's shaking noise.

[0048] Among them, the low-frequency motion component represents the low-frequency signal component that reflects the basic motion trajectory of the robot, and the frequency range is usually below 10Hz; the high-frequency vibration component represents the high-frequency noise signal caused by the vibration of the robot structure, and the frequency range is usually above 10Hz; the motion trajectory refers to the actual path of the robot's end effector in space; and the jitter noise represents the rapid oscillating displacement of the robot that deviates from the predetermined trajectory.

[0049] Specifically, the control system inputs the acquired actual joint angles into a preset low-pass filter to filter out high-frequency interference components and extract low-frequency motion components that reflect the robot's basic motion characteristics. The cutoff frequency of the preset low-pass filter needs to be set reasonably according to the robot's motion characteristics, ensuring that the motion signal is preserved without distortion while effectively filtering out vibration interference. Simultaneously, the control system inputs the actual vibration acceleration into a preset high-pass filter to extract high-frequency vibration components that reflect the robot's structural vibration characteristics. This signal separation processing provides targeted feedback information for subsequent trajectory control and vibration suppression.

[0050] Optionally, in general, the actual motion state data is processed to separate low-frequency motion components and high-frequency vibration components. The low-frequency motion components represent the robot's motion trajectory, and the high-frequency vibration components represent the robot's jitter noise. This can be achieved in the following ways, without limitation: input the actual joint angles into a preset low-pass filter, and output the robot's smooth angular trajectory as the low-frequency motion component; input the actual vibration acceleration into a preset high-pass filter, and output the robot's jitter acceleration data as the high-frequency vibration component.

[0051] Design of a preset low-pass filter:

[0052] (1) Filter type selection: Butterworth filter or Chebyshev filter is usually selected. Butterworth filter has the flattest passband response and smooth transition band; Chebyshev filter has a steeper cutoff characteristic, but the passband has ripple.

[0053] (2) Determination of cutoff frequency:

[0054] 1. Perform robot motion spectrum analysis: Collect FFT spectrum of normal motion data; analyze the frequency distribution of main motion components;

[0055] 2. Perform robot vibration spectrum analysis: Collect the FFT spectrum of vibration data; determine the starting frequency of vibration components;

[0056] 3. Set the cutoff frequency: fc_low=min (maximum motion frequency, vibration start frequency), usually between 5-10Hz.

[0057] (3) Filter order selection: Determine the filter order based on the stopband attenuation requirements.

[0058] Design of the preset high-pass filter:

[0059] (1) Filter type selection: Butterworth filter or Chebyshev filter can be selected. Phase characteristics need to be considered to avoid introducing excessive phase delay.

[0060] (2) Determination of cutoff frequency:

[0061] 1. Analyze the vibration spectrum: Collect vibration data and perform FFT analysis to determine the frequencies of the main vibration modes;

[0062] 2. Analyze the noise spectrum: Determine the frequency distribution of sensor noise;

[0063] 3. Set the cutoff frequency: fc_high=max (minimum vibration frequency, upper limit of motion frequency), usually between 10-20Hz.

[0064] Preset low-pass and high-pass filters can ensure effective separation of motion and vibration signals, providing a reliable data basis for subsequent control.

[0065] S104. Based on the low-frequency motion components, determine the actual motion pose of the robot, compare the actual motion pose with the theoretical motion pose to obtain the pose deviation. The theoretical motion pose is calculated from the theoretical motion response data.

[0066] The actual motion pose represents the spatial position and attitude angle of the robot's end effector relative to the base coordinate system, usually represented by a six-dimensional vector, including three position components and three attitude angles; the theoretical motion pose refers to the ideal pose data predicted by the robot's digital twin model; the pose deviation represents the difference between the actual motion pose and the theoretical motion pose, including position deviation and attitude deviation; the base coordinate system refers to the reference coordinate system fixed on the robot's base.

[0067] Specifically, the control system constructs a forward kinematics model based on the robot's link geometry parameters and DH parameters. This model describes the spatial relationships between the joint coordinate systems using a homogeneous transformation matrix. Then, the control system substitutes the joint angle data from the low-frequency motion components into the forward kinematics model and calculates the actual position coordinates and actual attitude angles of the robot's end effector relative to the base coordinate system through matrix multiplication. Simultaneously, the control system extracts the theoretical pose data for the corresponding moment from the theoretical motion response data. The control system calculates the difference between the actual and theoretical motion poses to obtain the pose deviation, which reflects the motion accuracy. This pose deviation includes position deviations in three directions and attitude deviations in three axes.

[0068] Optionally, in general, the actual motion pose of the robot can be determined based on the low-frequency motion components in the following ways, which are not limited here: obtain the robot's link geometry parameters and DH parameters to construct the robot's forward kinematics model; determine the smoothed angle data of each joint based on the low-frequency motion components, and substitute them into the forward kinematics model, and calculate the position coordinates and attitude angles of the robot's end effector relative to the base coordinate system through homogeneous transformation matrix multiplication; use the position coordinates and attitude angles as the actual motion pose.

[0069] The following detailed example illustrates the above process:

[0070] (1) First, determine the robot's DH parameter table (example parameters), as shown in the table below:

[0071]

[0072] Table 1 DH Parameter Table

[0073] (2) Assume the joint angle data (at a certain moment) obtained from the low-frequency motion components:

[0074]

[0075] Table 2 Joint Angle Data Table

[0076] (3) Calculate the homogeneous transformation matrix for each joint to determine the total transformation matrix:

[0077] T1=get_transform_matrix(0, -90, 495, 30.5);

[0078] T2=get_transform_matrix(450, 0, 0, -45.2);

[0079] T3=get_transform_matrix(0, 90, 0, 60.8);

[0080] T4=get_transform_matrix(0, -90, 420, -20.3);

[0081] T5=get_transform_matrix(0, 90, 0, 35.7);

[0082] T6=get_transform_matrix(0, 0, 80, 15.4);

[0083] T_actual = T1 @ T2 @ T3 @ T4 @ T5 @ T6;

[0084] (4) Extract position and orientation from the total transformation matrix to determine the robot’s actual motion pose.

[0085] S105. Perform energy analysis on the high-frequency vibration components to obtain the vibration energy density. Combine this with the preset vibration energy-stiffness mapping relationship to determine the stiffness coefficient used for vibration suppression.

[0086] Among them, vibration energy density represents the distribution of vibration energy within a unit frequency range; vibration energy-stiffness mapping relationship refers to the preset functional relationship that associates vibration energy with the required stiffness coefficient; stiffness coefficient represents the physical parameter describing the joint's resistance to deformation and determines the response characteristics of the control system to external disturbances.

[0087] Specifically, the control system performs a Fast Fourier Transform (FFT) on the high-frequency vibration signal of each joint to obtain the spectral characteristics of the high-frequency vibration signal. Then, the control system calculates the power spectral density function of the high-frequency vibration signal using autocorrelation analysis and the periodogram method. This power spectral density function reflects the distribution of vibration energy with frequency. The control system integrates the power spectral density function within the frequency band of interest to obtain the total energy value reflecting the vibration intensity. Based on a pre-established vibration energy-stiffness mapping relationship, the control system converts the calculated total energy value into a corresponding stiffness coefficient. This vibration energy-stiffness mapping relationship (established based on experience or experimental data) typically adopts a piecewise linear or exponential function form to ensure that stiffness is increased accordingly to enhance the suppression effect when vibration intensifies.

[0088] Optionally, under normal circumstances, energy analysis is performed on the high-frequency vibration components to obtain the vibration energy density. Combined with the preset vibration energy-stiffness mapping relationship, the stiffness coefficient used for vibration suppression can be determined in the following ways, which are not limited here: Power spectral density analysis is performed on the target high-frequency vibration components corresponding to the target joint to obtain the vibration energy distribution curve of the target joint, where the target joint is any joint; the integral value of the vibration energy distribution curve is calculated to obtain the total vibration energy of the target joint; according to the preset vibration energy-stiffness mapping relationship, the total vibration energy is mapped to the target stiffness coefficient of the target joint.

[0089] Assuming that the high-frequency vibration of the robot's third joint is mainly concentrated in two frequency bands: 40-60Hz with an energy of 17.0J and 110-130Hz with an energy of 5.0J, the total vibration energy is 22.0J, which is in the high-energy range. Based on the energy-stiffness mapping relationship, the stiffness coefficient is calculated to be 460N / m. This stiffness coefficient indicates that a large stiffness is required to suppress strong vibrations, which will be used for subsequent vibration suppression control.

[0090] S106. Based on the pose deviation, generate a position compensation command for trajectory correction and an impedance adjustment command for vibration suppression based on the stiffness coefficient.

[0091] Among them, the position compensation command refers to the supplementary control command used to correct the robot's motion trajectory; the impedance adjustment command refers to the supplementary control command used to adjust the mechanical impedance characteristics of the joint.

[0092] Specifically, the control system substitutes the pose deviation into the position compensation calculation formula, which includes proportional and derivative terms to simultaneously consider the effects of position and velocity errors. The values ​​of the proportional gain Kp and derivative gain Kd need to be determined experimentally to achieve good dynamic response characteristics while ensuring stability. For vibration suppression, the control system calculates the optimal damping coefficient based on the stiffness coefficient using the critical damping formula, and substitutes the relevant parameters into the second-order impedance model to generate joint torque commands for vibration suppression. These two types of control commands work together to ensure motion accuracy and provide effective vibration suppression.

[0093] Optionally, under normal circumstances, the position compensation command for trajectory correction can be generated based on the pose deviation in the following ways, which are not limited here: Substitute the pose deviation into the position compensation calculation formula to obtain the position compensation amount, and generate the position compensation command; calculate the impedance force based on the stiffness coefficient to generate the impedance adjustment command; the position compensation calculation formula is: ΔP=Kp×(Pd-Pa)+Kd×d(Pd-Pa) / dt; where ΔP is the position compensation amount, Pd is the theoretical motion pose, Pa is the actual motion pose, Kp is the position proportional gain, and Kd is the position differential gain.

[0094] Optionally, under normal circumstances, generating impedance adjustment commands for vibration suppression based on the stiffness coefficient can be achieved in the following ways, without limitation: Calculate the corresponding damping coefficient using the critical damping formula based on the stiffness coefficient; determine the joint angle deviation based on the pose deviation; calculate the difference between the theoretical joint angular velocity and the actual joint angular velocity to obtain the joint angular velocity deviation. The theoretical joint angular velocity is determined based on theoretical motion response data, and the actual joint angular velocity is determined based on actual motion state data; substitute the stiffness coefficient, damping coefficient, joint angle deviation, and joint angular velocity deviation into the second-order impedance model to calculate the joint torque to be compensated; use the joint torque as the impedance adjustment command; the second-order impedance model is: F = K × E + B × V; where F is the joint torque, K is the stiffness coefficient, E is the joint angle deviation, B is the damping coefficient, and V is the joint angular velocity deviation.

[0095] S107. Send the position compensation command and impedance adjustment command to the robot.

[0096] Specifically, first, the control system packages the position compensation command and impedance adjustment command according to a predetermined data format to ensure the integrity and correctness of the command data. Then, the control system sends the command data to the robot through a real-time communication interface. The communication protocol typically adopts industrial real-time bus standards such as EtherCAT and PROFINET to ensure the real-time performance and reliability of data transmission. After receiving the command data, the robot immediately updates the control parameters of each joint motor. The entire control process needs to be completed within one control cycle (usually 1-2 ms) to ensure real-time control. Through continuous command updates, the control system achieves continuous and precise control of the robot's movement.

[0097] By adopting the above technical solution, the control system constructs a digital twin model of the robot, determines the theoretical motion response data of the robot executing motor drive commands, and simultaneously collects the actual motion state data of the robot executing motor drive commands through equipment. The control system separates the actual motion state data into low-frequency motion components and high-frequency vibration components: the low-frequency motion component is used for trajectory correction, generating position compensation commands based on pose deviation; the high-frequency vibration component is used for vibration suppression, determining the stiffness coefficient through energy analysis and generating impedance adjustment commands. This dual feedback control mechanism ensures both the accuracy of the motion trajectory and the suppression of high-frequency vibrations, improving the robot's positioning accuracy and stability during high-speed movement. Through closed-loop control of theoretical motion response data and actual motion state data, precise control of the robot's motion is achieved, effectively solving the problem of insufficient vibration suppression capability of traditional PID control under high-speed conditions.

[0098] The following provides a more detailed description of the process of the method provided in this implementation. Please refer to [link / reference]. Figure 2 This is another flowchart illustrating the robot motion control method in this application.

[0099] The following steps may or may not be performed after step S107; this is not limited here:

[0100] S201. Obtain the corrected motion state data after the robot has executed the position compensation command and impedance adjustment command.

[0101] Among them, the corrected motion state data refers to the actual motion parameters of the robot after it performs compensation control, including joint angles, angular velocities, angular accelerations, as well as end-effector position, attitude, linear velocity, and angular velocity.

[0102] After the control system completes the compensation control in each control cycle (usually 1ms), it immediately acquires the robot's corrected motion state data. The acquisition steps can be found in step S102, and will not be repeated here.

[0103] S202. Calculate the residual deviation between the corrected motion state data and the theoretical motion response data.

[0104] Among them, residual error refers to the motion error that still exists after compensation control, including position error, attitude error and velocity error.

[0105] Specifically, the control system first synchronizes the corrected motion state data with the theoretical motion response data at the corresponding time; then, it calculates the position deviation vector, attitude deviation matrix, and velocity deviation vector respectively; finally, it calculates the comprehensive residual deviation index based on the weights of each type of deviation. The calculation process needs to consider the different units of measurement for position and attitude, and use appropriate normalization to make the various deviations comparable.

[0106] S203. When the residual deviation is greater than the preset model update threshold, the dynamic parameters of the robot digital twin model are corrected using the corrected motion state data. The dynamic parameters include at least the joint friction coefficient and the link mass distribution parameter.

[0107] Among them, the model update threshold refers to the critical deviation value that triggers the robot's digital twin model to update its parameters; dynamic parameters refer to physical quantities that describe the robot's dynamic characteristics, including joint friction coefficients (used to represent the resistance characteristics of joint movement) and link mass distribution parameters (used to represent the mass and inertial characteristics of each link of the robot); parameter correction refers to the process of adjusting the model parameters through optimization algorithms to make them closer to the actual physical system.

[0108] Specifically, the control system compares the calculated residual deviation with a preset model update threshold. When the residual deviation exceeds the threshold, a parameter identification algorithm is activated. This algorithm uses recently collected corrected motion state data to construct a least-squares optimization problem, solving for the updated joint friction coefficient and link mass distribution parameters. The updated parameters are then used for the dynamics calculation of the robot's digital twin model, enabling the model's behavior to more accurately reflect the robot's actual dynamic characteristics. The parameter correction process typically includes convergence checks to ensure that the parameter updates are stable and effective.

[0109] The control system in the embodiments of this invention is described below from the perspective of hardware processing. Please refer to [link / reference needed]. Figure 3 This is a schematic diagram of the physical device structure of the control system in an embodiment of this application.

[0110] It should be noted that, Figure 3 The structure of the control system shown is merely an example and should not impose any limitation on the functionality and scope of use of the embodiments of the present invention.

[0111] like Figure 3As shown, the control system includes a CPU 301, which can perform various appropriate actions and processes based on a program stored in the read-only memory ROM 302 or a program loaded from the storage section 308 into the random access memory RAM 303, such as executing the methods described in the above embodiments. The RAM 303 also stores various programs and data required for system operation. The CPU 301, ROM 302, and RAM 303 are interconnected via a bus 304. An I / O interface 305 is also connected to the bus 304.

[0112] The following components are connected to I / O interface 305: input section 306 including audio input devices, push-button switches, etc.; output section 307 including a liquid crystal display (LCD) and audio output devices, indicator lights, etc.; storage section 308 including a hard disk, etc.; and communication section 309 including a network interface card such as a LAN (Local Area Network) card, modem, etc. Communication section 309 performs communication processing via a network such as the Internet. Drive 310 is also connected to I / O interface 305 as needed. Removable media 311, such as a disk, optical disk, magneto-optical disk, semiconductor memory, etc., are installed on drive 310 as needed so that computer programs read from them can be installed into storage section 308 as needed.

[0113] In particular, according to embodiments of the present invention, the processes described above with reference to the flowcharts can be implemented as computer software programs. For example, embodiments of the present invention include a computer program product comprising a computer program carried on a computer-readable medium, the computer program containing computer programs for performing the methods shown in the flowcharts. In such embodiments, the computer program can be downloaded and installed from a network via communication section 309, and / or installed from removable medium 311. When the computer program is executed by CPU 301, it performs the various functions defined in the present invention.

[0114] It should be noted that specific examples of computer-readable storage media may include, but are not limited to: electrical connections having one or more wires, portable computer disks, hard disks, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM), flash memory, optical fiber, portable compact disc read-only memory (CD-ROM), optical storage devices, magnetic storage devices, or any suitable combination thereof. In this invention, a computer-readable storage medium can be any tangible medium containing or storing a program that can be used by or in conjunction with an instruction execution system, apparatus, or device.

[0115] The flowcharts and block diagrams in the accompanying drawings illustrate the architecture, functionality, and operation of possible implementations of systems, methods, and computer program products according to various embodiments of the present invention. Each block in a flowchart or block diagram may represent a module, program segment, or portion of code, which contains one or more executable instructions for implementing a specified logical function. It should also be noted that in some alternative implementations, the functions indicated in the blocks may occur in a different order than those shown in the drawings.

[0116] Specifically, the control system of this embodiment includes a processor and a memory. The memory stores a computer program, and when the computer program is executed by the processor, it implements the robot motion control method provided in the above embodiment.

[0117] In another aspect, the present invention also provides a computer-readable storage medium, which may be included in the control system described in the above embodiments; or it may exist independently and not incorporated into the control system. The storage medium carries one or more computer programs that, when executed by a processor of the control system, cause the control system to implement the robot motion control method provided in the above embodiments.

[0118] The above-described embodiments are only used to illustrate the technical solutions of this application, and are not intended to limit it. Although this application has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features. Such modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of this application.

[0119] As used in the above embodiments, depending on the context, the term "when..." can be interpreted as meaning "if...", "after...", "in response to determining...", or "in response to detecting...". Similarly, depending on the context, the phrase "when determining..." or "if (the stated condition or event) is interpreted as meaning "if determining...", "in response to determining...", "when (the stated condition or event) is detected", or "in response to detecting (the stated condition or event)".

[0120] Those skilled in the art will understand that all or part of the processes in the methods of the above embodiments can be implemented by a computer program instructing related hardware. This program can be stored in a computer-readable storage medium, and when executed, it can include the processes described in the above method embodiments. The aforementioned storage medium includes various media capable of storing program code, such as ROM or random access memory (RAM), magnetic disks, or optical disks.

Claims

1. A robot motion control method, characterized in that, Applied to a control system, the method includes: Send motor drive commands to the robot and input the motor drive commands into the robot's digital twin model to obtain the robot's theoretical motion response data. The motor drive commands are used to control the robot's joints to move along a predetermined trajectory. The robot's digital twin model is a virtual model that is pre-built based on the robot's dynamic and kinematic characteristics. The actual motion state data of the robot executing the motor drive command is obtained, and the actual motion state data includes the actual joint angles collected by the joint encoder and the actual vibration acceleration collected by the joint inertial sensor. The actual motion state data is processed to separate low-frequency motion components and high-frequency vibration components. The low-frequency motion components represent the robot's motion trajectory, and the high-frequency vibration components represent the robot's jitter noise. Based on the low-frequency motion components, the actual motion pose of the robot is determined, and the actual motion pose is compared with the theoretical motion pose to obtain the pose deviation. The theoretical motion pose is calculated from the theoretical motion response data. Energy analysis is performed on the high-frequency vibration components to obtain the vibration energy density. Combined with the preset vibration energy-stiffness mapping relationship, the stiffness coefficient used for vibration suppression is determined. Based on the pose deviation, a position compensation command for trajectory correction is generated, and based on the stiffness coefficient, an impedance adjustment command for vibration suppression is generated. The position compensation command and the impedance adjustment command are sent to the robot.

2. The method according to claim 1, characterized in that, The process of processing the actual motion state data to separate low-frequency motion components and high-frequency vibration components, wherein the low-frequency motion components represent the robot's motion trajectory and the high-frequency vibration components represent the robot's jitter noise, specifically includes: The actual joint angle is input into a preset low-pass filter, and the smooth angular trajectory of the robot is output as the low-frequency motion component. The actual vibration acceleration is input into a preset high-pass filter, and the output jitter acceleration data of the robot is used as the high-frequency vibration component.

3. The method according to claim 1, characterized in that, Determining the robot's actual motion pose based on the low-frequency motion components specifically includes: Obtain the link geometry parameters and DH parameters of the robot to construct the forward kinematics model of the robot; Based on the low-frequency motion components, the smooth angle data of each joint is determined and substituted into the forward kinematics model. Through the multiplication of homogeneous transformation matrices, the position coordinates and attitude angles of the robot's end effector relative to the base coordinate system are obtained. The position coordinates and the attitude angle are used as the actual motion pose.

4. The method according to claim 3, characterized in that, The energy analysis of the high-frequency vibration components yields the vibration energy density. Combined with a preset vibration energy-stiffness mapping relationship, the stiffness coefficient used for vibration suppression is determined, specifically including: Power spectral density analysis is performed on the high-frequency vibration components corresponding to the target joint to obtain the vibration energy distribution curve of the target joint, where the target joint is any joint; The total vibration energy of the target joint is obtained by calculating the integral value of the vibration energy distribution curve. Based on the preset vibration energy-stiffness mapping relationship, the total vibration energy is mapped to the target stiffness coefficient of the target joint.

5. The method according to claim 1, characterized in that, The step of generating a position compensation command for trajectory correction based on the pose deviation specifically includes: Substitute the pose deviation into the position compensation calculation formula to obtain the position compensation amount, and generate the position compensation command. Based on the stiffness coefficient, the impedance force is calculated to generate the impedance adjustment command; The formula for calculating the position compensation is: ΔP = Kp × (Pd - Pa) + Kd × d(Pd - Pa) / dt; Wherein, ΔP is the position compensation amount, Pd is the theoretical motion pose, Pa is the actual motion pose, Kp is the position proportional gain, and Kd is the position differential gain.

6. The method according to claim 5, characterized in that, The step of generating an impedance adjustment command for vibration suppression based on the stiffness coefficient specifically includes: Based on the stiffness coefficient, the corresponding damping coefficient is calculated using the critical damping formula; Based on the aforementioned pose deviation, the joint angle deviation is determined; The difference between the theoretical joint angular velocity and the actual joint angular velocity is calculated to obtain the joint angular velocity deviation. The theoretical joint angular velocity is determined based on the theoretical motion response data, and the actual joint angular velocity is determined based on the actual motion state data. Substitute the stiffness coefficient, the damping coefficient, the joint angle deviation, and the joint angular velocity deviation into the second-order impedance model to calculate the joint torque that needs to be compensated. The joint torque is used as the impedance adjustment command; The second-order impedance model is: F = K × E + B × V; Wherein, F is the joint torque, K is the stiffness coefficient, E is the joint angle deviation, B is the damping coefficient, and V is the joint angular velocity deviation.

7. The method according to claim 1, characterized in that, After the step of sending the position compensation command and the impedance adjustment command to the robot, the method further includes: Acquire the corrected motion state data of the robot after it has executed the position compensation command and the impedance adjustment command; Calculate the residual deviation between the corrected motion state data and the theoretical motion response data; When the residual deviation is greater than the preset model update threshold, the dynamic parameters of the robot digital twin model are corrected using the corrected motion state data. The dynamic parameters include at least the joint friction coefficient and the link mass distribution parameter.

8. A control system, characterized in that, The control system includes: one or more processors and a memory; the memory is coupled to the one or more processors, the memory is used to store computer program code, the computer program code including computer instructions, and the one or more processors call the computer instructions to cause the control system to perform the method as described in any one of claims 1-7.

9. A computer-readable storage medium comprising instructions, characterized in that, When the instruction is executed on the control system, it causes the control system to perform the method as described in any one of claims 1-7.

10. A computer program product, characterized in that, When the computer program product is run on the control system, the control system performs the method as described in any one of claims 1-7.

Images