Robot motion predictive control method based on ros2 asynchronous feedback fusion

Abstract

The application discloses a robot motion prediction control method based on ROS2 asynchronous feedback fusion, and particularly relates to the field of robot control and automation technology, and adopts a heterogeneous dual-cycle structure of software and hardware cooperation, realizes accurate time axis alignment of high and low layer cross-frequency asynchronous data through hardware level time stamp and dynamic sliding window, establishes a dynamics prediction model containing an uncertainty compensation vector, uses a prediction residual of a last control cycle to update a compensation amount on line through weighted filtering, and combines interpolation and extrapolation double modes to enhance jitter fault tolerance, further constructs a quadratic programming performance index based on the aligned fusion state, solves an optimal control sequence under motor dynamic torque and anti-saturation constraints, and implements pre-adjustment closed-loop control by taking the first control amount as a feedforward correction amount, so that the application effectively eliminates asynchronous communication delay, and significantly improves robot trajectory tracking accuracy, response speed and anti-interference ability.

Description

Technical Field

[0001] This invention relates to the field of robot control and automation technology, and more specifically, to a robot motion prediction control method based on ROS2 asynchronous feedback fusion. Background Technology

[0002] With the rapid development of robotics technology, ROS2, due to its excellent modularity and cross-platform characteristics, is widely used in robot motion planning and system scheduling. In a typical ROS2 robot control system, ROS2 acts as a middleware layer, undertaking trajectory planning and system scheduling functions. Its built-in data distribution service communication model adopts a publish-subscribe mechanism, which is suitable for loosely coupled data interaction between heterogeneous nodes. Data is sent to the underlying control card via network communication, and the control card then drives servo motors to execute motion at a high frequency cycle.

[0003] However, in actual industrial and service scenarios, traditional control methods have the following significant drawbacks:

[0004] Asynchronous communication leads to data temporal and spatial misalignment: ROS2 distributes data based on DDS, which is a non-strict real-time system, and the communication process is subject to random network jitter; while the control card is a high-frequency strict real-time system. When encoder feedback data is asynchronously transmitted to the ROS2 layer, there is a non-negligible time difference with the expected trajectory of the higher layer. Directly comparing the errors will cause control jitter.

[0005] Hysteresis response and dynamic error propagation: Traditional closed-loop control relies heavily on post-error compensation, meaning that correction is only performed after deviation from the desired trajectory. During high-speed or high-precision motion, this hysteresis can easily cause joint vibration in the robot, or even trigger safety protection and abruptly stop the robot due to accumulated errors. Summary of the Invention

[0006] To overcome the aforementioned deficiencies of the prior art, embodiments of the present invention provide a robot motion prediction and control method based on ROS2 asynchronous feedback fusion to solve the problems mentioned in the background art.

[0007] To achieve the above objectives, the present invention provides the following technical solution: a robot motion prediction and control method based on ROS2 asynchronous feedback fusion, comprising the following steps:

[0008] S1. Establish a robot dynamics prediction model, which is used to predict the joint state sequence in the future time domain based on the current joint state and control input; the prediction model includes an uncertainty compensation vector, which is updated on a rolling basis according to the deviation between the actual feedback and the predicted value in the previous control cycle.

[0009] S2. The high-level planning layer of the control system generates the desired trajectory in the first cycle and sends the trajectory command to the control card execution layer; the control card execution layer drives the servo motor to run in the second cycle, which is less than the first cycle, and injects a hardware-level timestamp when collecting encoder feedback data.

[0010] S3. The high-level planning layer receives encoder feedback data reported by the control card execution layer through asynchronous communication middleware, and uses sliding window and interpolation to accurately align the encoder feedback data and the desired trajectory on the time axis, and constructs a feedback fusion state vector based on the aligned feedback data and the desired trajectory.

[0011] S4. Input the feedback fusion state vector generated in S3 into the prediction model, combine the control increment constraint and the joint limit constraint, perform rolling optimization solution in the prediction time domain, calculate the future control sequence, and use the first control quantity of the control sequence as the feedforward correction compensation quantity to pre-adjust the trajectory command before the deviation occurs.

[0012] S5. At the control card execution layer, the feedforward correction compensation amount and the high-frequency expected control command are fedforward superimposed and fused to generate the final discrete control command to drive the robot to execute the corrected motion, thereby realizing feedforward motion prediction closed-loop control. When the error value in the feedback fusion state vector exceeds the preset threshold, the penalty weight of the tracking error in the prediction model is dynamically increased. When the error continues to expand beyond the preset number of times, the control system switches to the fault protection mode.

[0013] Preferably, as a preferred embodiment of the robot motion prediction and control method based on ROS2 asynchronous feedback fusion described in this invention, S1 specifically includes the following:

[0014] Based on the URDF model parameters of the robot, the rigid body dynamics model of the robot is constructed using the Lagrange method: ,in, For the robot joint position vector With joint velocity vector The resulting continuous joint state vector To control the input vector, Let be the robot's nominal dynamic state function. Represents the total uncertainty term;

[0015] Using the second period as the sampling period, the continuous-time state-space equation is discretized, and the continuous state vector... With control input vector The sampled values ​​in the k-th control cycle are denoted as follows: and ;

[0016] Establish a dynamic prediction model: ,in, Let be the state vector of the kth control cycle. To control the input vector, and These are the state transition matrix and the control input matrix, respectively. This is the uncertainty compensation vector corresponding to the k-th control cycle;

[0017] In the k-th control cycle, the following compensation vector update steps are performed:

[0018] Extract the state prediction value output by the k-1 prediction model from the previous control cycle. And combined with the feedback fusion state vector of the current k-th control cycle The prediction residual for the previous control period is calculated as follows: ;in, The residual vector is predicted; positive values ​​indicate that the actual state is higher than the predicted value, and negative values ​​indicate that the actual state is lower than the predicted value.

[0019] The uncertainty compensation vector for the current period is obtained through a weighted filtering algorithm. Online rolling updates: ;in, The preset residual gain matrix, The historical compensation attenuation coefficient is the historical compensation attenuation coefficient. An adaptive dynamic adjustment strategy is adopted: when the rate of change of the predicted residual... When the rate of change exceeds a preset threshold, automatically reduce The value is adjusted to improve the response speed to sudden disturbances; when the rate of change of the predicted residual is less than or equal to a preset rate of change threshold, the value is automatically increased. Values ​​are added to enhance filter smoothness;

[0020] Let the prediction time domain length be N, and let the feedback fusion state vector of the current k-th control cycle be used. As the initial state for prediction If the feedback fusion state vector has not yet been generated in the current control cycle. Then, the original feedback state vector directly collected by the control card execution layer is used instead; the updated uncertainty compensation vector is used. The dynamic prediction model is injected, and the joint state prediction sequence for the next N control cycles is recursively solved. The specific formula is as follows:

[0021] ;

[0022] in, It is the transpose symbol. To predict the state sequence matrix, This represents the joint state vector predicted for the Nth future control cycle, starting from the current kth control cycle.

[0023] When predicting residuals Continuously exceeding the preset threshold Upon reaching the preset cycle, the real-time current and torque values ​​of the servo motor are synchronously verified: if the actual current or torque exceeds the limit, a physical fault is identified and the fault protection mode is triggered; if the actual current and torque are normal, the uncertainty compensation vector is reset. .

[0024] Preferably, as a preferred embodiment of the robot motion prediction and control method based on ROS2 asynchronous feedback fusion according to the present invention, it includes the following in step S2:

[0025] High-level planning level in The first cycle Generate the desired trajectory node vector for the current period. And it is sent to the control card execution layer via the real-time bus; the control card execution layer uses a second cycle that is less than the first cycle. Drive the servo motor to run, where, , ,and It is a positive integer;

[0026] The control card execution layer in each second cycle The control timer interrupt triggers the hardware capture register to automatically record the current high-precision hardware timestamp. It also synchronously reads the original position and speed sampling data of each joint encoder; the control card execution layer is based on the high-precision timestamp of the hardware. The original sampled data is compensated for communication delay and phase deviation to generate an original feedback state vector with high-precision hardware timestamps. The original feedback state vector is then reported to the asynchronous communication middleware in the higher-level planning layer.

[0027] The control card execution layer in two consecutive first cycles Between the expected trajectory nodes, in the second cycle The step size is generated online using a polynomial interpolation algorithm. A high-frequency desired control command sequence ,in, Indicates the first The first cycle; This indicates the m-th second cycle moment within the first cycle, to ensure the smoothness and absence of abrupt changes in the servo motor drive signal;

[0028] When the control card execution layer detects packet loss or communication delay exceeding a preset security threshold in higher-level communication, it automatically switches to the extrapolation interpolation mode based on the dynamic prediction model, and uses the state sequence of the previous cycle and the uncertainty compensation vector to generate the temporary expected instruction for the current second cycle online.

[0029] The control card execution layer will use the original feedback state vector With high-frequency expected control command sequence With the second cycle Real-time input is fed into the robot dynamics prediction model to realize high-level trajectory planning and low-level servo control in the second cycle. High-frequency synchronous prediction and uncertainty compensation.

[0030] Preferably, as a preferred embodiment of the robot motion prediction and control method based on ROS2 asynchronous feedback fusion described in this invention, step S3 specifically includes the following:

[0031] The high-level planning layer receives high-precision hardware timestamps via ROS2 asynchronous communication middleware based on the DDS communication mechanism. encoder feedback data And store them sequentially in the opened circular sliding window according to the time sequence;

[0032] Based on the expected trajectory node timestamps of the current high-level planning layer Based on this, search for the condition that satisfies the following conditions within the sliding window. adjacent encoder feedback data frames and The corresponding calculation was obtained using interpolation. Feedback fusion state vector at time step ;

[0033] When the sliding window is affected by asynchronous communication jitter When the preset communication cycle threshold is exceeded, it is determined that packet loss or high latency has occurred, and the system automatically switches to the extrapolation alignment mode based on the robot dynamics prediction model; using the last received frame of raw feedback data within the window. Starting from this point, and combining it with the current uncertainty compensation vector Based on the dynamic prediction model, the current desired trajectory node timestamp is estimated by recursion. The feedback state is used to obtain the synthesized result, which is the aligned feedback fusion state vector. The specific formula is as follows:

[0034] ;

[0035] in, The timestamp of the last valid feedback data frame received within the sliding window. and Let be the state transition matrix and the control input matrix of the k-th control cycle of the dynamic prediction model, respectively. for of Power of; In the first The control input vector for each control cycle For the number of steps that need to be extrapolated, The rounding up symbol; This is the uncertainty compensation vector that remains unchanged during the extrapolation process;

[0036] Preset maximum allowed extrapolation step threshold When the number of consecutive extrapolation steps The system was determined to be experiencing a serious communication failure and was put into a safe deceleration and shutdown mode.

[0037] When the sliding window receives normal feedback data for a preset number of frame intervals, it automatically switches from extrapolation alignment mode back to interpolation alignment mode; within the preset transition period after the switch, a weighted transition attenuation factor is used to perform weighted smooth fusion of the interpolated calculated value and the extrapolated predicted value.

[0038] and the interpolated calculation Or obtained by extrapolation estimation The feedback fusion state vector for the current k-th control cycle is denoted as... This serves as the predicted initial state for the kth control cycle in the current round S4, and also as the predicted initial state for the (k+1)th control cycle in the next round S1.

[0039] Preferably, as a preferred embodiment of the robot motion prediction and control method based on ROS2 asynchronous feedback fusion described in this invention, step S4 specifically includes the following:

[0040] The feedback fusion state vector generated in S3 As the initial state for the current prediction, construct the prediction time domain. The performance index function of the quadratic programming within the interval is given by the following formula:

[0041] ;

[0042] in, To control the increment vector, For the expected sequence of future states, This is the state tracking error weight matrix. To control the incremental smoothing weight matrix, To control the energy weighting matrix;

[0043] In the k-th control cycle, the performance index function is evaluated under the following set of physical constraints. Perform rolling solution:

[0044] Joint kinematic and dynamic limits: ;in, and These are the lower and upper bound vectors for each joint state, respectively. This represents the joint state vector predicted in the i-th step;

[0045] Controlling incremental amplitude limits: ; used to limit the rate of change of torque between two adjacent control cycles, where, and These are the lower and upper bound vectors for controlling the increment, respectively. This represents the control increment at step i;

[0046] Control quantity saturation constraint: This is used to limit the absolute amplitude of the control quantity from exceeding the rated output capability of the servo driver; where, and These are the lower limit vector and upper limit vector of the control quantity, respectively. This represents the control input vector at step i;

[0047] Preferably, the joint limit constraint further includes a dynamic torque upper limit constraint based on the servo motor speed-torque characteristics: in the multi-step prediction recursion, the upper limit of the control quantity corresponding to the current step is dynamically updated according to the predicted real-time joint speed to prevent the control command from exceeding the limit due to the motor back electromotive force limitation under high-speed operation.

[0048] In the control quantity saturation constraint, the rotational speed is predicted in real time based on the joint. Update using the following function: ;

[0049] When the speed is lower than the rated base speed, the upper limit of the output maintains the rated peak torque. When the rotational speed exceeds the rated base speed, the upper limit of the control quantity is set according to... Proportional decay, where, This is the rated base speed of the motor; The rated peak torque vector, This represents the upper limit of the current control quantity after dynamic decay. Let be the joint rotational speed predicted in step i;

[0050] The above constrained optimization problem is solved using a quadratic programming solver to obtain the optimal control sequence for the next N control cycles: ;

[0051] Extract the first control vector of the optimal control sequence. As the feedforward correction compensation amount for the current cycle .

[0052] Preferably, as a preferred embodiment of the robot motion prediction and control method based on ROS2 asynchronous feedback fusion described in this invention, S5 specifically includes the following:

[0053] The feedforward correction compensation amount output by S4 High-frequency desired control commands generated by S2 Feedforward overlay and fusion are performed at the control card execution layer to generate the final discrete control command. During the superposition process, a feedforward anti-saturation check is performed: when it is determined that the final control command after superposition exceeds the maximum output capacity of the driver, the original trajectory command output is maintained, and the feedforward correction compensation is reduced proportionally. The internal state integral term of the quadratic programming solver is envelope-restricted; and the final discrete control command is used. Drive the servo motor to run;

[0054] Real-time calculation of the feedback fusion state vector Joint position error norm With velocity error norm ;

[0055] When detected Exceeding the preset position error threshold or Exceeding the preset speed error threshold At that time, the state tracking error weight matrix in the quadratic programming performance index function is adaptively increased. Dynamic adjustment: ,in, The reference state tracking error weight matrix is... This is a preset weight surge factor;

[0056] Construct a continuous error amplification counter Detect error status within the sliding monitoring window: when the error norm continuously satisfies The counter increments as time goes on. When the error converges When the counter is reset, it is cleared. ;

[0057] When the counter reaches the preset consecutive count threshold When this happens, the control system immediately switches to fault protection mode, which includes:

[0058] Level 1 protection: When the current joint speed is lower than the preset safe speed threshold, the high-level planning layer immediately generates a damped deceleration trajectory based on acceleration limit constraints and executes controlled smooth deceleration and shutdown.

[0059] Level 2 protection: The current joint speed exceeds the preset safe speed threshold or the error mutation amplitude exceeds the safe limit and continues to exceed it. If the control cycle still does not converge, the control card directly sends a hardware-level safety torque shutdown signal and a brake command to the servo driver to cut off the servo motor drive torque and implement mechanical braking.

[0060] On the other hand, the present invention provides a computer device including a memory and a processor, wherein the memory stores a computer program, wherein when the computer program is executed by the processor, it implements the functional modules of the robot motion prediction control method based on ROS2 asynchronous feedback fusion as described above.

[0061] On the other hand, the present invention provides a computer-readable storage medium having a computer program stored thereon, wherein: when the computer program is executed by a processor, it implements the robot motion prediction control method based on ROS2 asynchronous feedback fusion as described above in the present invention.

[0062] The technical effects and advantages provided by the present invention in the above technical solution are as follows:

[0063] By employing a heterogeneous dual-cycle architecture with hardware and software collaboration and a hardware-level timestamp alignment mechanism, the transmission delay and phase shift caused by cross-frequency asynchronous communication are effectively eliminated. An uncertainty compensation vector based on the rolling update of the prediction residual of the previous control cycle is introduced into the dynamic prediction model. Combined with adaptive weighted filtering and extrapolation degradation protection, the trajectory tracking accuracy and system robustness under unmodeled dynamics and jitter interference are significantly improved. By combining quadratic programming rolling optimization with dynamic torque upper limit and anti-saturation verification, feedforward pre-adjustment closed-loop control is achieved under the premise of ensuring the safety of the motor's physical limits. It has the engineering advantages of advanced response, smooth and shock-free operation, and high fault tolerance and anti-divergence. Attached Figure Description

[0064] To more clearly illustrate the technical solutions in the embodiments of this application or the prior art, the drawings used in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments recorded in this invention. For those skilled in the art, other drawings can be obtained based on these drawings.

[0065] Figure 1 This is an overall flowchart of the motion prediction control method in an embodiment of the present invention;

[0066] Figure 2This is a schematic diagram illustrating the principle of high-low layer dual-cycle scheduling and precise time axis alignment in an embodiment of the present invention;

[0067] Figure 3 This is a schematic diagram illustrating the uncertainty compensation and quadratic programming rolling optimization principle of the dynamic prediction model in this embodiment of the invention;

[0068] Figure 4 This is a schematic diagram comparing the trajectory tracking error of the method in this embodiment of the invention with that of a traditional control method under communication jitter. Detailed Implementation

[0069] The technical solutions of the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of this application, and not all embodiments. Based on the embodiments of this application, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this application.

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

[0071] In the description of this application, the term "for example" is used to mean "used as an example, illustration, or description." Any embodiment described as "for example" in this application is not necessarily to be construed as being more preferred or advantageous than other embodiments. The following description is provided to enable any person skilled in the art to make and use the invention. Details are set forth in the following description for purposes of explanation. It should be understood that those skilled in the art will recognize that the invention can be made without using these specific details. In other instances, well-known structures and processes will not be described in detail to avoid obscuring the description of the invention with unnecessary detail. Therefore, the invention is not intended to be limited to the embodiments shown, but is consistent with the broadest scope of the principles and features disclosed in this application.

[0072] Example 1

[0073] This embodiment provides, for example Figure 1 The robot motion prediction and control method based on ROS2 asynchronous feedback fusion, as shown, specifically includes the following steps:

[0074] S1. Establish a robot dynamics prediction model, which is used to predict the joint state sequence in the future time domain based on the current joint state and control input; the prediction model includes an uncertainty compensation vector, which is updated on a rolling basis according to the deviation between the actual feedback and the predicted value in the previous control cycle.

[0075] S2. The high-level planning layer of the control system generates the desired trajectory in the first cycle and sends the trajectory command to the control card execution layer; the control card execution layer drives the servo motor to run in the second cycle, which is less than the first cycle, and injects a hardware-level timestamp when collecting encoder feedback data.

[0076] S3. The high-level planning layer receives encoder feedback data reported by the control card execution layer through asynchronous communication middleware, and uses sliding window and interpolation to accurately align the encoder feedback data and the desired trajectory on the time axis, and constructs a feedback fusion state vector based on the aligned feedback data and the desired trajectory.

[0077] S4. Input the feedback fusion state vector generated in S3 into the prediction model, combine the control increment constraint and the joint limit constraint, perform rolling optimization solution in the prediction time domain, calculate the future control sequence, and use the first control quantity of the control sequence as the feedforward correction compensation quantity to pre-adjust the trajectory command before the deviation occurs.

[0078] S5. At the control card execution layer, the feedforward correction compensation amount and the high-frequency expected control command are fedforward superimposed and fused to generate the final discrete control command to drive the robot to execute the corrected motion, thereby realizing feedforward motion prediction closed-loop control. When the error value in the feedback fusion state vector exceeds the preset threshold, the penalty weight of the tracking error in the prediction model is dynamically increased. When the error continues to expand beyond the preset number of times, the control system switches to the fault protection mode.

[0079] Preferably, in step S1, a robot dynamics prediction model is established. This prediction model is used to predict the joint state sequence in the future time domain based on the current joint state and control input. The prediction model includes an uncertainty compensation vector, which is updated on a rolling basis according to the deviation between the actual feedback and the predicted value in the previous control cycle. Specifically, it includes the following:

[0080] Based on the URDF model parameters of the robot, the rigid body dynamics model of the robot is constructed using the Lagrange method: ,in, For the robot joint position vector With joint velocity vector The resulting continuous joint state vector To control the input vector, Let be the robot's nominal dynamic state function. This represents the total uncertainty term of the system, which is not modeled for friction, load variations, and external disturbances.

[0081] Using the second period as the sampling period, the continuous-time state-space equations are discretized to construct a dynamic prediction model that includes an uncertainty compensation vector: ,in, and Representing the continuous joint state vectors respectively With control input vector The sampled value in the k-th control cycle, and Based on the current state The linearized state transition matrix and the control input matrix; preferably, the state transition matrix... With control input matrix Based on the current joint angle in the kth control cycle With joint velocity The calculated Jacobian matrix is ​​updated in real time. This is the uncertainty compensation vector corresponding to the k-th control cycle, used to compensate for model mismatch and external disturbances;

[0082] In the k-th control cycle, the following compensation vector update steps are performed:

[0083] Extract the state prediction value output by the k-1 prediction model from the previous control cycle. , representing the predicted state for the current k-th control cycle, starting from the previous control cycle k−1, and combined with the feedback fusion state vector of the current k-th control cycle. The prediction residual for the previous control period is calculated as follows: ;in, The residual vector is predicted; positive values ​​indicate that the actual state is higher than the predicted value, and negative values ​​indicate that the actual state is lower than the predicted value.

[0084] Using predicted residuals The uncertainty compensation vector for the current period is obtained through a weighted filtering algorithm. Online rolling updates: ;in, The preset residual gain matrix, This is the historical compensation attenuation coefficient, used to control the weight distribution between historical compensation terms and the current predicted residual. Preferably, , The larger the value, the more historical compensation items are retained, and the smoother the compensation update. The historical compensation attenuation coefficient... An adaptive dynamic adjustment strategy is adopted: when the rate of change of the predicted residual... When the rate of change exceeds a preset threshold, automatically reduce The value is adjusted to improve the response speed to sudden disturbances; when the rate of change of the predicted residual is less than or equal to a preset rate of change threshold, the value is automatically increased. The value is adjusted to enhance the smoothness of the filter and prevent the system from generating high-frequency jitter.

[0085] Let the prediction time domain length be N, and let the feedback fusion state vector of the current k-th control cycle be used. As the initial state for prediction If the feedback fusion state vector has not yet been generated in the current control cycle. Then, the original feedback state vector directly collected by the control card execution layer is used instead; the updated uncertainty compensation vector is used. The dynamic prediction model is injected, and the joint state prediction sequence for the next N control cycles is recursively solved. The specific formula is as follows:

[0086] ;

[0087] in, It is the transpose symbol. To predict the state sequence matrix, This represents the joint state vector predicted for the Nth future control cycle, starting from the current kth control cycle. During the multi-step prediction recursion process, the uncertainty compensation vector is extrapolated to a constant value in the future prediction time domain. Assuming that model mismatch and external disturbances remain unchanged in the future prediction time domain,

[0088] Preferably, when the robot system is first started, the uncertainty compensation vector is initialized to a zero vector: To prevent abnormal divergence of the uncertainty compensation vector due to instantaneous sensor noise or sudden external disturbances, the following measures are taken: Each element is subject to amplitude limiting:

[0089] ;

[0090] in, Uncertainty compensation vector The j-th component, This is a standard limiting function used to constrain input values ​​to a specified upper and lower limit range; The maximum allowable compensation amplitude for the preset j-th state component is preferably... Take 5% to 15% of the rated driving force;

[0091] When predicting residuals Continuously exceeding the preset threshold When a preset number of control cycles are reached, the uncertainty compensation vector will be automatically reset. To avoid the continuous accumulation and loss of control of compensation terms due to sensor failure or continuous abnormal disturbance;

[0092] Before triggering the uncertainty compensation vector reset, the real-time current and torque values ​​of the servo motor collected by the control card are synchronously verified: if the actual current or torque exceeds the limit, it is determined to be a physical overload or a real collision, and the fault protection mode is directly triggered; if the actual current and torque are normal, it is determined to be a feedback signal disturbance or calculation mismatch, and only the uncertainty compensation vector reset is executed to ensure the continuous and smooth operation of the robot without downtime.

[0093] Preferably, in step S2, the high-level planning layer of the control system generates the desired trajectory in a first cycle and sends the trajectory command to the control card execution layer; the control card execution layer drives the servo motor to run in a second cycle shorter than the first cycle, and injects a hardware-level timestamp when collecting encoder feedback data, specifically including the following:

[0094] Let the first period be The second cycle is And satisfy ,in ,and It is a positive integer; the high-level planning layer is at the th . The first cycle Generate the desired trajectory node vector for the current period. And it is sent to the control card execution layer via the real-time bus; preferably, the first cycle The second cycle lasts from 1ms to 5ms. The duration is 100μs to 500μs;

[0095] The control card execution layer in each second cycle The control timer interrupt triggers the hardware capture register to automatically record the current high-precision hardware timestamp. It also synchronously reads the original position and speed sampling data of each joint encoder; the control card execution layer is based on the high-precision timestamp of the hardware. The original sampled data is compensated for communication delay and phase deviation to generate an original feedback state vector with high-precision hardware timestamps. The original feedback state vector The actual position and velocity of each joint after time deviation compensation, along with its high-precision hardware timestamp, are reported to the asynchronous communication middleware in the high-level planning layer as input data for subsequent timestamp alignment and feedback fusion.

[0096] The control card execution layer in two consecutive first cycles Between the expected trajectory nodes, in the second cycle The step size is generated online using a polynomial interpolation algorithm. A high-frequency desired control command sequence ,in, Indicates the first At the m-th moment of the second cycle within the first cycle. To ensure the smoothness and absence of abrupt changes in the servo motor drive signal; preferably, when the control card execution layer detects packet loss or communication delay exceeding a preset safety threshold in the higher-level communication, it automatically switches to the extrapolation interpolation mode based on the dynamic prediction model, and uses the state sequence of the previous cycle and the uncertainty compensation vector to generate the temporary expected command of the current second cycle online, thus ensuring the continuity of motion;

[0097] The control card execution layer will use the original feedback state vector With high-frequency expected control command sequence With the second cycle Real-time input is fed into the robot dynamics prediction model to realize high-level trajectory planning and low-level servo control in the second cycle. High-frequency synchronous prediction and uncertainty compensation.

[0098] Preferably, in step S3, the high-level planning layer receives encoder feedback data reported by the control card execution layer through an asynchronous communication middleware, and uses a sliding window and interpolation method to accurately align the encoder feedback data and the desired trajectory on the time axis, and constructs a feedback fusion state vector based on the aligned feedback data and the desired trajectory; specifically including the following:

[0099] The asynchronous communication middleware is preferably a ROS2 robot operating system based on the DDS communication mechanism; preferably, the ROS2 robot operating system adopts a zero-copy transmission mechanism based on shared memory and a "Best Effort" low-latency transmission strategy, and directly extracts the hardware-level high-precision timestamp injected by the control card execution layer from the ROS2 message header. Eliminate transmission jitter and latency caused by the operating system's network protocol stack;

[0100] The higher-level planning layer allocates memory for a specified time period. The circular sliding window will receive the data, which includes the time zone and a high-precision hardware timestamp. encoder feedback data Stored sequentially in the sliding window according to time order;

[0101] Based on the expected trajectory node timestamps of the current high-level planning layer Based on this, two adjacent encoder feedback data frames are retrieved within the sliding window. and And satisfy The corresponding calculation was obtained using interpolation. Feedback fusion state vector at time step Preferably, before performing interpolation alignment, the high-level planning layer uses weighted least squares to estimate the minute clock drift rate between the high-level system clock and the control card hardware clock in real time based on the clock deviation of multiple consecutive frames of data within the sliding window, and performs high-precision timestamps on the hardware. Compensation is performed to eliminate the cumulative alignment error caused by the misalignment of the two-terminal crystal oscillator;

[0102] The interpolation method employs a cubic Hermitian interpolation algorithm based on acceleration continuity constraints, generating an alignment state according to the position and velocity data of the sampling points at both ends; when the sliding window experiences asynchronous communication jitter... When the preset communication cycle threshold is exceeded, it is determined that packet loss or high latency has occurred. The system automatically switches to the extrapolation alignment mode based on the robot dynamics prediction model, utilizing... The state at time step and the current uncertainty compensation vector are recursively estimated forward. Real-time feedback status ensures continuous alignment accuracy of the high-level planning layer under asynchronous network disturbances;

[0103] The extrapolation alignment mode is suitable for scenarios where there are insufficient data frames within the sliding window due to communication jitter, making interpolation alignment impossible. In the extrapolation alignment mode, the last received frame of original feedback data within the window is used. Starting from this point, and combining it with the current uncertainty compensation vector Based on the dynamic prediction model, the current desired trajectory node timestamp is estimated by recursion. The feedback state is used to obtain the synthesized result, which is the aligned feedback fusion state vector. The specific formula is as follows:

[0104] ;

[0105] in, The timestamp of the last valid feedback data frame received within the sliding window. and Let be the state transition matrix and the control input matrix of the k-th control cycle of the dynamic prediction model, respectively. for of Power of 1 For Starting with the original feedback data at each moment, the results are obtained through extrapolation using a prediction model. The fused state vector is fed back at any time and compared with the one obtained in the interpolation mode. It is used in the same way in subsequent steps; In the first The control input vector for each control cycle The number of steps required for extrapolation indicates the number of steps from... Time's up The number of extrapolated second cycle steps required at each time point. The floor function is the floor function, indicating that the smallest integer not less than the given value is taken. For the first The uncertainty compensation vector for each control cycle is obtained by keeping the current estimate unchanged during extrapolation. Through the above extrapolation and recursion, continuous alignment feedback can still be provided even when communication is interrupted or delayed, avoiding control interruption due to missing feedback data; and the interpolated calculations are then used to calculate the... Or obtained by extrapolation estimation The feedback fusion state vector for the current k-th control cycle is denoted as... , and serve as the predicted initial state for the kth control cycle in the current round S4, and the predicted initial state for the (k+1)th control cycle in the next round S1;

[0106] Preferably, a maximum allowed extrapolation step threshold is preset. When the number of consecutive extrapolation steps If a serious communication failure is detected, the system will automatically switch to a safe deceleration and shutdown mode to prevent the cumulative estimation error from diverging due to unlimited extrapolation.

[0107] Preferably, after receiving normal feedback data for a preset number of frame intervals within the sliding window, the system automatically switches from extrapolation alignment mode back to interpolation alignment mode; during the initial transition from extrapolation alignment mode to interpolation alignment mode... Within each control cycle, a weighted transition attenuation factor is introduced to smoothly fuse the interpolated and extrapolated predicted values, eliminating the control command jumps and mechanical shocks caused by sudden changes in state estimation during mode switching.

[0108] Preferably, in step S4, the feedback fusion state vector generated in step S3 is input into the prediction model. Combining control increment constraints and joint limit constraints, rolling optimization is performed in the prediction time domain to calculate the future control sequence. The first control variable of the control sequence is used as the feedforward correction compensation variable to pre-adjust the trajectory command before a deviation occurs. Specifically, this includes the following:

[0109] The feedback fusion state vector generated in S3 As the initial state for the current prediction, construct the prediction time domain. The performance index function of the quadratic programming within the interval is given by the following formula:

[0110] ;

[0111] in, To control the increment vector, For the expected sequence of future states, This is the state tracking error weight matrix. To control the incremental smoothing weight matrix, To control the energy weighting matrix;

[0112] In the k-th control cycle, the performance index function is evaluated under the following set of physical constraints. Perform rolling solution:

[0113] Joint kinematic and dynamic limits: ;in, and These are the lower and upper bound vectors for each joint state, including angle limits, velocity limits, and acceleration limits. This represents the joint state vector predicted in step i, containing the positions of each joint. ,speed and acceleration ;

[0114] Controlling incremental amplitude limits: Used to limit the rate of torque change between two adjacent control cycles, preventing sudden command changes from causing mechanical shocks and high-frequency oscillations. and These are the lower and upper bound vectors for controlling the increment, respectively. This represents the control increment at step i, used to eliminate high-frequency jitter in the control command and avoid driver overshoot;

[0115] Control quantity saturation constraint: This is used to limit the absolute amplitude of the control quantity from exceeding the rated output capacity of the servo driver, preventing motor overcurrent or driver overload tripping; among which, and These are the lower and upper limit vectors of the control quantity, respectively, corresponding to the rated output capability range of the servo driver. This represents the control input vector at step i;

[0116] Preferably, the joint limit constraint further includes a dynamic torque upper limit constraint based on the servo motor speed-torque characteristics: in the multi-step prediction recursion, the upper limit of the control quantity corresponding to the current step is dynamically updated according to the predicted real-time joint speed to prevent the control command from exceeding the limit due to the motor back electromotive force limitation under high-speed operation.

[0117] In the control quantity saturation constraint, the rotational speed is predicted in real time based on the joint. Update using the following function: ;

[0118] When the speed is lower than the rated base speed, the upper limit of the output maintains the rated peak torque. When the rotational speed exceeds the rated base speed, the upper limit of the control quantity is set according to... Proportional attenuation is used to suppress control command overshoot caused by back EMF limitations under high-speed operating conditions. This is the rated base speed of the motor; The rated peak torque vector, This represents the upper limit of the current control quantity after dynamic decay. Let be the joint rotational speed predicted in step i;

[0119] The above constrained optimization problem is solved using a quadratic programming solver to obtain the optimal control sequence for the next N control cycles: ;

[0120] Extract the first control vector of the optimal control sequence. As the feedforward correction compensation amount for the current cycle .

[0121] Preferably, in step S5, the feedforward correction compensation amount and the high-frequency expected control command are fed forward and fused at the control card execution layer to generate the final discrete control command to drive the robot to execute the corrected motion, thereby realizing feedforward motion prediction closed-loop control; when the error value in the feedback fusion state vector exceeds a preset threshold, the penalty weight of the tracking error in the prediction model is dynamically increased; when the error continues to expand beyond a preset number of times, the control system switches to fault protection mode; specifically including the following:

[0122] The feedforward correction compensation amount output by S4 High-frequency desired control commands generated by S2 Feedforward overlay and fusion are performed at the control card execution layer to generate the final discrete control command. During the superposition process, a feedforward anti-saturation check is performed: when it is determined that the final control command after superposition exceeds the maximum output capacity of the driver, the original trajectory command output is maintained, and the feedforward correction compensation is reduced proportionally. The internal state integral term of the quadratic programming solver is envelope-restricted; and the final discrete control command is used. Drive the servo motor to run;

[0123] Real-time calculation of the feedback fusion state vector Joint position error norm With velocity error norm ;

[0124] When detected Exceeding the preset position error threshold or Exceeding the preset speed error threshold At that time, the tracking error penalty weight matrix in the quadratic programming performance index function is adaptively increased. Dynamic adjustment: ,in, The reference state tracking error weight matrix is... The preset weight surge factor is preferably taken as follows: An adaptive exponential jump function proportional to the error over-limit value is adopted. By significantly increasing the penalty for tracking errors, the deviation in the prediction time domain is forcibly reduced, thus accelerating error convergence.

[0125] Construct a continuous error amplification counter : Allocate a sliding error monitoring window in memory for a specified number of steps; when the error norm is continuously satisfied... This means that the counter increments when both the error exceeds the limit and the rate of change of the error is greater than zero. When the error converges When the counter is reset, it is cleared. This is to eliminate misjudgments caused by instantaneous high-frequency noise from the sensor;

[0126] When the counter reaches the preset consecutive count threshold When the robot's motion is determined to be systematically divergent or subject to severe external interference, the control system immediately switches to fault protection mode.

[0127] Preferably, the fault protection mode includes a graded degradation security protection mechanism:

[0128] The first level of protection is dynamic smooth controlled deceleration. When the current joint speed is lower than the safe speed threshold, the high-level planning layer immediately generates a damped deceleration trajectory based on acceleration limit constraints. The control card continues to execute feedforward prediction closed-loop control until the robot stops smoothly.

[0129] Level 2 protection is hardware-level dynamic braking and emergency stop, which occurs when the current joint speed exceeds the safe speed threshold or the error mutation exceeds the safe limit and continues for an extended period. If the control cycle still has not converged, the control card directly sends a hardware-level safety torque shutdown signal and a dynamic brake command to the servo driver to cut off the servo motor drive torque and implement mechanical braking to prevent mechanical collisions and equipment damage.

[0130] In this embodiment, the control system used to execute the above-mentioned motion prediction control method is a robot real-time control software and hardware system based on the ROS2 architecture, specifically including a high-level planning layer and a control card execution layer running on an industrial computer or embedded processor.

[0131] The high-level planning layer is a trajectory planning and scheduling software module running in the ROS2 environment, which is responsible for executing high-level method steps such as generating the desired trajectory, precise alignment of the time axis, and secondary planning and rolling optimization; the control card execution layer is a low-level servo control hardware or firmware module based on a real-time operating system, which is responsible for receiving high-frequency commands to drive the servo motor and injecting hardware-level timestamps when collecting encoder feedback data.

[0132] The computer device can be a terminal, comprising a processor, memory, communication interface, display screen, and input devices connected via a system bus. The processor provides computing and control capabilities. The memory includes non-volatile storage media and internal memory. The non-volatile storage media stores the operating system and computer programs. The internal memory provides an environment for the operation of the operating system and computer programs stored in the non-volatile storage media. The communication interface is used for wired or wireless communication with external terminals; wireless communication can be achieved through Wi-Fi, carrier networks, NFC (Near Field Communication), or other technologies. The display screen can be an LCD screen or an e-ink screen. The input devices can be a touch layer covering the display screen, buttons, a trackball, or a touchpad on the computer device's casing, or an external keyboard, touchpad, or mouse.

[0133] This embodiment also provides a storage medium storing a computer program that, when executed by a processor, implements the steps of the robot motion prediction control method based on ROS2 asynchronous feedback fusion as proposed in the above embodiments. The storage medium can be implemented by any type of volatile or non-volatile storage device or a combination thereof, such as Static Random Access Memory (SRAM), Electrically Erasable Programmable Read-Only Memory (EEPROM), Erasable Programmable Read Only Memory (EPROM), Programmable Red-Only Memory (PROM), Read-Only Memory (ROM), magnetic storage, flash memory, magnetic disk, or optical disk.

[0134] Example 2

[0135] The following is another embodiment of the present invention, which provides a robot motion prediction control method based on ROS2 asynchronous feedback fusion. In order to verify the beneficial effects of the present invention, a simulation experiment is conducted for scientific demonstration.

[0136] This experiment aims to verify the effectiveness of the robot motion prediction control method based on ROS2 asynchronous feedback fusion. By employing techniques such as high-low layer heterogeneous dual-cycle communication, hardware-level timestamp injection, sliding window time axis alignment, uncertainty compensation vector rolling update, and quadratic programming rolling optimization, the experiment improves the robot's trajectory tracking accuracy and real-time response under asynchronous communication latency and unmodeled disturbances. The experiment uses simulated and actual collected robot joint motion data, including joint trajectory characteristics, timestamp deviation, uncertainty compensation amount, feedforward correction amount, and state tracking error under different communication jitter and external load conditions. By analyzing the conformity between the model's predicted trajectory and the actual expected trajectory, the accuracy and robustness of the invention in eliminating communication latency and resisting disturbances are verified.

[0137] The simulation experiment steps are implemented according to the content of the robot motion prediction control method based on ROS2 asynchronous feedback fusion provided in Example 1, and the specific steps include:

[0138] A six-DOF industrial robotic arm model was constructed in the ROS2 simulation environment, and the first cycle of the high-level planning layer was set. Control card execution layer second cycle And in the interrupt service routine of the control card, the injection of high-precision hardware timestamps and the acquisition of encoder feedback data are simulated.

[0139] An uncertainty compensation vector is introduced into the robot dynamics prediction model. It is updated on a rolling basis according to the deviation between the actual feedback state and the predicted value in the previous control cycle. Different degrees of model parameter mismatch and external load disturbance are simulated in the simulation process to verify the adaptive adjustment capability of the compensation vector.

[0140] The high-level planning layer issues the desired trajectory command through the ROS2 DDS communication mechanism. The control card execution layer performs polynomial interpolation between two consecutive first cycle nodes with the second cycle as the step size to generate M high-frequency desired control command sequences. In the simulation, periodic communication delay and random packet loss conditions are set to test the switching logic and alignment accuracy of sliding window interpolation and extrapolation alignment modes.

[0141] The feedback fusion state vector constructed after timestamp alignment is input into the prediction model. Combined with joint position limits, velocity limits and control increment constraints, the rolling optimization solution is performed. The first control quantity is used as the feedforward correction compensation quantity and superimposed on the trajectory command of the next cycle to drive the robot to execute the corrected motion.

[0142] The system monitors and provides feedback on the position error in the fused state vector in real time. When the error exceeds a preset threshold, the system dynamically increases the penalty weight of the tracking error in the prediction model. When the error continues to expand beyond a preset number of times, the system is triggered to switch to fault protection mode.

[0143] The specific data from the above simulation experiment are shown in Table 1.

[0144] Table 1 Data Recording Table of Simulation Experiment of the Invention

[0145] Time / milliseconds Planning layer / control card cycle Communication latency and jitter Feedback data alignment mode Uncertainty Compensation Vector Norm Feedforward correction compensation amount / N·m Tracking error / mm Weight matrix state Control system operating status 0-10 150 (normal) Interpolation Alignment 0.02 (Initial value convergence) 0.05 0.001 (extremely low) benchmark Normal closed-loop control 10-20 320 (mild shaking) Interpolation Alignment 0.15 (Response Load) 0.42 0.003 (tiny) benchmark Dynamic smoothing compensation 20-30 1200 (Sudden packet loss) Extrapolation Alignment 0.38 (continuous extrapolation) 0.85 0.008 (controllable) Dynamic surge Dynamic anti-interference / extrapolation 30-40 180 (Returned to normal) Smooth fusion switching 0.22 (Smooth decay) 0.35 0.002 (rapid convergence) Restoration benchmark Smooth switchback in interpolation mode 40-50 4500 (Continuous Interruption) Extrapolation Exceeded Limit 1.20 (Triggered packet limiting) 1.50 (Resistant to saturation decay) 0.035 (Sudden increase exceeding limit) exponential surge Triggering fault protection / degraded shutdown

[0146] Experimental Analysis:

[0147] To verify the accuracy, real-time performance, and robustness of the robot motion prediction control method based on ROS2 asynchronous feedback fusion described in this invention, this embodiment constructs a high-low layer heterogeneous cross-frequency control architecture and simulates different network communication disturbance environments. By comparing the model predicted trajectory, actual expected trajectory conformity, and response characteristics of the method of this invention with traditional control methods under different communication disturbances, the results are plotted as follows: Figure 4 The diagram shows a comparison of trajectory tracking errors.

[0148] This embodiment compares the trajectory tracking errors of the two methods under different time periods and communication conditions. (Unit: mm) Continuous monitoring and comparative analysis were conducted, and the details are as follows:

[0149] During the normal communication phase (0-10ms): network communication latency is within the normal range, and the trajectory tracking error of the method of this invention differs from that of traditional methods. All maintained within the normal tracking error tolerance. Within this range, the robot's movement exhibits extremely high smoothness;

[0150] Network jitter and sudden packet loss phase 10-30ms: When mild communication jitter occurs within 10-20ms and sudden packet loss occurs within 20-30ms:

[0151] (1) Traditional control method: Due to the lack of timing alignment and predictive extrapolation mechanism, the control command and feedback data are mismatched, resulting in trajectory tracking error. It diverged violently and rose sharply, quickly exceeding the adaptive trigger threshold. It even approaches the extreme value of the allowable tracking error. ;

[0152] (2) The method of the present invention: by starting the sliding window alignment, and automatically and smoothly switching the feedback data alignment mode from "interpolation" to "extrapolation" mode during the 20-30ms packet loss phase; combined with the solved feedforward correction compensation amount Real-time injection is performed, from 0.05 Dynamically adjusted to 0.85 This causes the trajectory tracking error The fluctuations were only slight, around 0.008 mm, consistently well below the trigger threshold. With the extreme value of the allowable tracking error ;

[0153] Communication recovery and smooth switchback phase (30-40ms): After communication is restored, the method of this invention, based on adaptive weight adjustment, smoothly switches the data alignment mode back to the interpolation mode. The uncertainty compensation vector norm steadily decreases from 0.38 to 0.22, reducing the trajectory tracking error. It converges rapidly to 0.002mm, avoiding the mechanical shock caused by sudden data changes during network recovery, which is common in traditional methods.

[0154] Long-term interruption extreme protection phase 40-50ms: When a sustained and severe communication interruption occurs:

[0155] (1) Traditional control method: The error bar directly breaks through the extreme value of the allowable tracking error. Uncontrolled dispersion can easily lead to mechanical collisions or overload failures.

[0156] (2) The method of the present invention: real-time monitoring of extrapolation timeout and error exceeding the limit The trend automatically triggers a graded fault protection strategy, and the feedforward quantity implements anti-saturation attenuation and packet limiting to reduce trajectory tracking errors. Strictly controlled at the safety limit Internally, and implement a downgraded safety shutdown;

[0157] In summary, the experiments show that the robot motion prediction control method based on ROS2 asynchronous feedback fusion has high tracking accuracy, real-time response and anti-interference ability, and can effectively ensure the smoothness and safety of robot motion throughout its entire life cycle under a high-low layer heterogeneous cross-frequency control architecture.

[0158] The foregoing has only described certain exemplary embodiments of the present invention by way of illustration. Undoubtedly, those skilled in the art can modify the described embodiments in various ways without departing from the spirit and scope of the present invention. Therefore, the foregoing drawings and descriptions are illustrative in nature and should not be construed as limiting the scope of protection of the claims of the present invention.

Claims

1. A robot motion prediction and control method based on ROS2 asynchronous feedback fusion, characterized in that: Specifically, the following steps are included: S1. Establish a robot dynamics prediction model, which is used to predict the joint state sequence in the future time domain based on the current joint state and control input; the prediction model includes an uncertainty compensation vector, which is updated on a rolling basis according to the deviation between the actual feedback and the predicted value in the previous control cycle. S2. The high-level planning layer of the control system generates the desired trajectory in the first cycle and sends the trajectory command to the control card execution layer; the control card execution layer drives the servo motor to run in the second cycle, which is less than the first cycle, and injects a hardware-level timestamp when collecting encoder feedback data. S3. The high-level planning layer receives encoder feedback data reported by the control card execution layer through asynchronous communication middleware, and uses sliding window and interpolation to accurately align the encoder feedback data and the desired trajectory on the time axis, and constructs a feedback fusion state vector based on the aligned feedback data and the desired trajectory. S4. Input the feedback fusion state vector generated in S3 into the prediction model, combine the control increment constraint and the joint limit constraint, perform rolling optimization solution in the prediction time domain, calculate the future control sequence, and use the first control quantity of the control sequence as the feedforward correction compensation quantity to pre-adjust the trajectory command before the deviation occurs. S5. At the control card execution layer, the feedforward correction compensation amount and the high-frequency expected control command are fedforward superimposed and fused to generate the final discrete control command to drive the robot to execute the corrected motion. When the error value in the feedback fusion state vector exceeds a preset threshold, the penalty weight for tracking error in the prediction model is dynamically increased. When the error continues to expand beyond a preset number of times, the control system switches to fault protection mode.

2. The robot motion prediction and control method based on ROS2 asynchronous feedback fusion according to claim 1, characterized in that: Specifically, S1 includes the following: Establish a dynamic prediction model: ,in, Let be the state vector of the kth control cycle. To control the input vector, and These are the state transition matrix and the control input matrix, respectively. This is the uncertainty compensation vector for the k-th control cycle; In the k-th control cycle, the following compensation vector update steps are performed: Extract the state prediction value output by the k-1 prediction model from the previous control cycle. And combined with the feedback fusion state vector of the current k-th control cycle The prediction residual for the previous control period is calculated as follows: ;in, To predict the residual vector; The uncertainty compensation vector for the current k-th control cycle is obtained through a weighted filtering algorithm. Online rolling updates: ;in, The preset residual gain matrix, This is the historical compensation attenuation coefficient; Let the prediction time domain length be N, and let the feedback fusion state vector of the current k-th control cycle be used. As the initial state for prediction If the feedback fusion state vector has not yet been generated in the current control cycle. Then, the original feedback state vector directly collected by the control card execution layer is used instead; the updated uncertainty compensation vector is used. The dynamic prediction model is injected, and the joint state prediction sequence for the next N control cycles is recursively solved. The specific formula is as follows: ； in, It is the transpose symbol. To predict the state sequence matrix, This represents the joint state vector predicted for the Nth future control cycle, starting from the current kth control cycle.

3. The robot motion prediction and control method based on ROS2 asynchronous feedback fusion according to claim 2, characterized in that: The historical compensation attenuation coefficient An adaptive dynamic adjustment strategy is adopted: when predicting the rate of change of residuals When the rate of change exceeds a preset threshold, automatically reduce Value; when the rate of change of the predicted residual is less than or equal to the preset rate of change threshold, it is automatically increased. value; When the predicted residual continuously exceeds the preset threshold Upon reaching the preset cycle, the real-time current and torque values ​​of the servo motor are synchronously verified: if the actual current or torque exceeds the limit, a physical fault is identified and the fault protection mode is triggered; if the actual current and torque are normal, the uncertainty compensation vector is reset. .

4. The robot motion prediction and control method based on ROS2 asynchronous feedback fusion according to claim 1, characterized in that: Specifically, S2 includes the following: High-level planning level in the first cycle The desired trajectory nodes are issued and transmitted to the control card execution layer via the real-time bus; the control card execution layer executes in the second cycle. Running, among which, , ,and It is a positive integer; The control card execution layer in each second cycle The control timer interrupt triggers the hardware capture register to automatically record the current high-precision hardware timestamp. And generate the original feedback state vector with high-precision hardware timestamps. The report is then sent to the asynchronous communication middleware in the high-level planning layer. The control card execution layer in two consecutive first cycles Between the expected trajectory nodes, in the second cycle The step size is generated online using a polynomial interpolation algorithm. A high-frequency desired control command sequence ,in, Indicates the first The first cycle; This indicates the m-th second cycle moment within the first cycle, to ensure the smoothness and absence of abrupt changes in the servo motor drive signal; When the control card execution layer detects packet loss or communication delay exceeding a preset security threshold in higher-level communication, it automatically switches to the extrapolation interpolation mode based on the dynamic prediction model, and uses the state sequence of the previous cycle and the uncertainty compensation vector to generate the temporary expected instruction for the current second cycle online. The control card execution layer will use the original feedback state vector With high-frequency expected control command sequence With the second cycle Real-time input is fed into the robot dynamics prediction model to realize high-level trajectory planning and low-level servo control in the second cycle. High-frequency synchronous prediction and uncertainty compensation.

5. The robot motion prediction and control method based on ROS2 asynchronous feedback fusion according to claim 1, characterized in that: Specifically, S3 includes the following: The high-level planning layer receives high-precision hardware timestamps via ROS2 asynchronous communication middleware based on the DDS communication mechanism. encoder feedback data And store them sequentially in the opened circular sliding window according to the time sequence; Based on the expected trajectory node timestamps of the current high-level planning layer Based on this, search for the condition that satisfies the following conditions within the sliding window. adjacent encoder feedback data frames and The corresponding calculation was obtained using interpolation. Feedback fusion state vector at time step ; When the sliding window is affected by asynchronous communication jitter When the preset communication cycle threshold is exceeded, switch to extrapolation alignment mode; use the last received frame of raw feedback data within the window. Starting from this point, and combining it with the current uncertainty compensation vector Based on the dynamic prediction model, the current desired trajectory node timestamp is estimated by recursion. The feedback state is used to obtain the synthesized result, which is the aligned feedback fusion state vector. The specific formula is as follows: ； in, The timestamp of the last valid feedback data frame received within the sliding window. and Let be the state transition matrix and the control input matrix of the k-th control cycle of the dynamic prediction model, respectively. for of Power of; In the first The control input vector for each control cycle For the number of steps that need to be extrapolated, The rounding up symbol; This is the uncertainty compensation vector that remains unchanged during the extrapolation process; and the interpolated calculation Or obtained by extrapolation estimation The feedback fusion state vector for the current k-th control cycle is denoted as... This serves as the predicted initial state for the kth control cycle in the current round S4, and also as the predicted initial state for the (k+1)th control cycle in the next round S1.

6. The robot motion prediction and control method based on ROS2 asynchronous feedback fusion according to claim 5, characterized in that: In S3, a maximum allowed extrapolation step threshold is preset. ; When the number of consecutive extrapolation steps The system was determined to be experiencing a serious communication failure and was put into a safe deceleration and shutdown mode. When feedback data with a normal time interval of a preset number of frames is received continuously within the sliding window, the system automatically switches from extrapolation alignment mode back to interpolation alignment mode. During the preset transition period after the switch, a weighted transition attenuation factor is used to perform weighted smooth fusion of the interpolated calculated value and the extrapolated predicted value.

7. The robot motion prediction control method based on ROS2 asynchronous feedback fusion according to claim 1, characterized in that: Specifically, S4 includes the following: The feedback fusion state vector generated in S3 As the initial state for the current prediction, construct the prediction time domain. The performance index function of the quadratic programming within the interval is given by the following formula: ； in, To control the increment vector, For the expected sequence of future states, This is the state tracking error weight matrix. To control the incremental smoothing weight matrix, To control the energy weighting matrix; In the k-th control cycle, the performance index function is evaluated under the following set of physical constraints. Perform rolling solution: Joint kinematic and dynamic limits: ;in, and These are the lower and upper bound vectors for each joint state, respectively. This represents the joint state vector predicted in the i-th step; Controlling incremental amplitude limits: ; used to limit the rate of change of torque between two adjacent control cycles, where, and These are the lower and upper bound vectors for controlling the increment, respectively. This represents the control increment at step i; Control quantity saturation constraint: This is used to limit the absolute amplitude of the control quantity from exceeding the rated output capability of the servo driver; where, and These are the lower limit vector and upper limit vector of the control quantity, respectively. This represents the control input vector at step i; The above constrained optimization problem is solved using a quadratic programming solver to obtain the optimal control sequence for the next N control cycles: ; Extract the first control vector of the optimal control sequence. As the feedforward correction compensation amount for the current k-th control cycle .

8. The robot motion prediction and control method based on ROS2 asynchronous feedback fusion according to claim 7, characterized in that: In the control quantity saturation constraint, the rotational speed is predicted in real time based on the joint. Update using the following function: ; When the speed is lower than the rated base speed, the upper limit of the output maintains the rated peak torque. ; When the rotational speed exceeds the rated base speed, the upper limit of the control quantity is set according to... Proportional decay, where, This is the rated base speed of the motor; This represents the upper limit of the current control quantity after dynamic decay. Let be the joint rotational speed predicted in step i.

9. The robot motion prediction and control method based on ROS2 asynchronous feedback fusion according to claim 1, characterized in that: Specifically, S5 includes the following: The feedforward correction compensation amount output by S4 With high-frequency expected control commands Feedforward overlay and fusion are performed at the control card execution layer to generate the final discrete control command. During the superposition process, a feedforward anti-saturation check is performed: when it is determined that the final control command after superposition exceeds the maximum output capacity of the driver, the original trajectory command output is maintained, and the feedforward correction compensation is reduced proportionally. The internal state integral term of the quadratic programming solver is envelope-restricted; and the final discrete control command is used. Drive the servo motor to run; Real-time calculation of feedback fusion state vector Joint position error norm With velocity error norm ; When detected Exceeding the preset position error threshold or Exceeding the preset speed error threshold At that time, the state tracking error weight matrix in the quadratic programming performance index function is adaptively increased. Dynamic adjustment: ,in, The reference state tracking error weight matrix is... This is a preset weight surge factor; Construct a continuous error amplification counter Detect error status within the sliding monitoring window: when the error norm continuously satisfies The counter increments as time goes on. When the error converges When the counter is reset, it is cleared. ; When the counter reaches the preset consecutive count threshold When this happens, the control system immediately switches to fault protection mode, which includes: Level 1 protection: When the current joint speed is lower than the preset safe speed threshold, the high-level planning layer immediately generates a damped deceleration trajectory based on acceleration limit constraints and executes controlled smooth deceleration and shutdown. Level 2 protection: The current joint speed exceeds the preset safe speed threshold or the error mutation amplitude exceeds the safe limit and continues to exceed it. If the control cycle still has not converged, a hardware-level safety torque shutdown signal and a brake command are issued to cut off the servo motor drive torque and implement mechanical braking.

Images