Method and device for online identification and compensation of robot arm load based on extended kalman filter

Abstract

This invention provides a method and apparatus for online load identification and compensation of a robotic arm based on extended Kalman filtering. The method includes: collecting joint torque data of multiple configurations of the robotic arm under no-load conditions to construct a no-load torque benchmark library; collecting measured joint torques of the robotic arm under load, and separating the pure load torque caused by the end-load load by combining the no-load torque benchmark library; based on extended Kalman filtering, using a state vector containing load mass parameters and center of gravity offset parameters as the object to be identified, and the pure load torque as the observed variable, identifying the current load mass and center of gravity offset parameters online through iterative calculations of prediction and update steps; using the identified load mass and center of gravity offset parameters as dynamic variables to correct the kinematic and dynamic models of the robotic arm in real time; and performing torque compensation control on the robotic arm based on the corrected kinematic and dynamic models.

Description

Technical Field

[0001] This application relates to the field of robot control, specifically to a method and apparatus for online identification and compensation of load on a robotic arm based on extended Kalman filtering. Background Technology

[0002] Conventional kinematic and dynamic modeling of collaborative robotic arms is based on the ideal working condition where the load center of gravity coincides with the center of the flange at the end of the robotic arm. In actual production scenarios, the end of the robotic arm needs to frequently change various irregularly shaped workpieces. The irregular shape and uneven mass distribution of the workpieces can easily cause the load center of gravity at the end to deviate from the flange center, generating eccentric loads and additional torques. This directly affects the dynamic force balance of the robotic arm across its entire range and reduces the matching accuracy between the dynamic model and the actual working condition.

[0003] To address the issues of load center of gravity shift and eccentric load caused by frequent changes in irregularly shaped workpieces at the end effector of collaborative robotic arms, existing load compensation solutions mainly suffer from the following drawbacks:

[0004] 1) Currently, the main focus is on designing corresponding compensation algorithms for ideal center loads. The kinematic and dynamic models have fixed parameters, and the models do not reserve adaptation interfaces for off-center load conditions, resulting in poor adaptability.

[0005] 2) It mainly uses offline calibration, which cannot identify and dynamically correct the eccentricity in real time. Changing the load requires stopping the machine for debugging. It cannot adapt to the needs of flexible production lines for uninterrupted operation and rapid changeover, resulting in low work efficiency.

[0006] 3) Existing solutions have a simple compensation logic, which only corrects the end-effector pose deviation at the kinematic level. They do not combine the load eccentricity characteristics to identify and optimize the joint dynamic torque parameters simultaneously. They fail to compensate for the additional inertial torque and gravitational torque disturbance caused by eccentric load from the root, and the compensation effect is limited.

[0007] 4) It cannot accurately identify the magnitude of the eccentricity and the direction of the center of gravity offset. It adopts a uniform compensation force and has no differentiated adjustment capability for different degrees of eccentric load. It is very easy to overcompensate, resulting in positioning overshoot, or undercompensate, resulting in substandard accuracy. Summary of the Invention

[0008] To address the aforementioned problems, this invention provides a method and apparatus for online identification and compensation of robotic arm load based on extended Kalman filtering.

[0009] The technical solution of this invention is as follows:

[0010] This invention provides a method for online load identification and compensation of a robotic arm based on extended Kalman filtering, the method comprising:

[0011] Collect joint torque data of the robotic arm in multiple configurations under no-load conditions, and build a no-load torque benchmark library;

[0012] Collect the measured joint torques of the robotic arm during loaded operation, and combine them with the unloaded torque reference library to separate the pure load torque caused by the end load;

[0013] Based on extended Kalman filtering, the state vector containing load mass parameters and center of gravity offset parameters is taken as the object to be identified, and the pure load torque is taken as the observed variable. Through iterative calculations of prediction step and update step, the current load mass and center of gravity offset parameters are identified online.

[0014] The identified load mass and center of gravity offset parameters are used as dynamic variables to correct the kinematic and dynamic models of the robotic arm in real time.

[0015] Based on the modified kinematic and dynamic models, torque compensation control is performed on the robotic arm.

[0016] Preferably, the step of separating the pure load torque caused by the end load includes:

[0017] The collected load torque data is averaged and filtered for noise reduction.

[0018] Outlier removal is performed on the filtered load torque data;

[0019] Interpolate the no-load torque reference library based on the current joint attitude angle to obtain the no-load torque that is aligned with the loaded torque pose;

[0020] The pure load torque is obtained by subtracting the processed loaded torque from the interpolated unloaded torque.

[0021] Preferably, the step of identifying the current load quality and center of gravity offset parameters online through iterative calculations of the prediction step and the update step includes:

[0022] Establish the load eccentricity state vector;

[0023] In the prediction step, the state prior estimate for the current time is calculated based on the state posterior estimate identified at the previous time step; the covariance prior estimate for the current time step is calculated based on the covariance posterior estimate and the process noise covariance matrix at the previous time step; and the theoretical predicted torque is calculated based on the state prior estimate and the nonlinear observation model at the current time step.

[0024] In the update step, the torque residual is calculated based on the measured pure load torque and the theoretically predicted torque; a first-order Taylor expansion is performed on the nonlinear observation function at the state prior estimate to obtain the observation Jacobian matrix; the Kalman gain is calculated using the covariance prior estimate at the current time, the observation Jacobian matrix, and the observation noise covariance matrix; the state prior estimate at the current time is corrected using the torque residual and the Kalman gain to obtain the state posterior estimate at the current time; the covariance posterior estimate at the current time is updated using the Kalman gain, the observation Jacobian matrix, and the covariance prior estimate at the current time; the state posterior estimate is output as the identification result at the current time and input as the state posterior estimate for the next prediction step; the covariance posterior estimate is input as the covariance posterior estimate for the next prediction step.

[0025] Calculate the Kalman gain, and use the torque residual and Kalman gain to correct the state prior estimate to obtain the state posterior estimate at the current time.

[0026] The posterior state estimate is output as the identification result at the current time and used as the input for the prediction step at the next time.

[0027] Preferably, when the extended Kalman filter is first started, the initial eccentricity parameter is retrieved from the preset workpiece parameter reference library as the initial value for iteration.

[0028] Preferably, the method further includes:

[0029] Set a deviation threshold and define the deviation value;

[0030] When the torque residual is greater than the deviation threshold for multiple consecutive frames, it is determined to be a model deviation, the extended Kalman filter update is paused, and the parameter fine-tuning of the nonlinear observation model is initiated.

[0031] When the torque residual is less than or equal to the deviation threshold for multiple consecutive frames, it is determined to be observation noise, and the extended Kalman filter iterates normally.

[0032] Preferably, the step of real-time correction of the robotic arm's dynamic model includes:

[0033] Based on the DH parameters of the robotic arm and the identified center of gravity offset parameters, an eccentric correction Jacobian matrix that is adaptive to the current eccentric working condition is constructed to replace the original fixed Jacobian matrix, and the end reference point is moved from the flange center to the actual load center of gravity.

[0034] Preferably, the step of real-time correction of the robotic arm's dynamic model includes:

[0035] The load inertia tensor is recalculated based on the identified center of gravity offset parameters to replace the fixed constant inertia tensor when the load center of gravity coincides with the flange center.

[0036] Based on the identified eccentricity parameters, the real-time force load and eccentric additional torque amplitude of each joint are calculated, and the joint friction coefficient is adaptively adjusted accordingly.

[0037] Preferably, the method further includes:

[0038] Calculate the radial eccentricity based on the identified load mass and center of gravity offset parameters;

[0039] The torque compensation force and the upper limit of the movement speed are automatically adjusted according to the magnitude of the radial eccentricity.

[0040] Preferably, the automatic adjustment of the torque compensation force and the upper limit of the movement speed based on the magnitude of the radial eccentricity includes:

[0041] When the radial eccentricity is less than the first preset distance, a micro-eccentricity compensation mode with small-amplitude correction of kinematic and dynamic parameters is adopted.

[0042] When the radial eccentricity is greater than or equal to the first preset distance and less than the second preset distance, a small eccentricity compensation mode with full synchronous correction and superimposed torque compensation is adopted.

[0043] When the radial eccentricity is greater than or equal to the second preset distance, a large eccentricity compensation mode is adopted, which strengthens torque compensation and reduces the upper limit of the motion speed to a set ratio of the rated speed.

[0044] The present invention also provides a robotic arm load online identification and compensation device based on extended Kalman filtering, the device comprising:

[0045] The module is used to collect joint torque data of the robotic arm in multiple configurations under no-load conditions and build a no-load torque benchmark library.

[0046] The separation module is used to collect the measured joint torques of the robotic arm when it is under load, and combine them with the unloaded torque reference library to separate the pure load torque caused by the end load.

[0047] The identification module is used to identify the current load mass and center of gravity offset parameters online based on extended Kalman filtering, using a state vector containing load mass parameters and center of gravity offset parameters as the object to be identified, and the pure load torque as the observed variable, through iterative calculations of prediction step and update step.

[0048] The correction module is used to use the identified load mass and center of gravity offset parameters as dynamic variables to correct the kinematic and dynamic models of the robotic arm in real time.

[0049] The compensation module is used to perform torque compensation control on the robotic arm based on the modified kinematic and dynamic models.

[0050] The beneficial effects of this application are as follows:

[0051] Without increasing hardware costs, requiring offline manual calibration, or relying on external auxiliary tooling, this method uses algorithms to identify the end-effector load mass and center of gravity position online. Based on the identification results, it performs real-time corrections to load-related parameters in the kinematic, dynamic, and nonlinear observation models, achieving accurate online sensing of load eccentricity and center of gravity shift. This method can effectively suppress the disturbances of complex eccentric loads on the system at the control level, improve the positioning accuracy and operational stability of the robotic arm under high-speed and variable-posture conditions, and adapt to the needs of flexible production lines for uninterrupted operation and rapid changeover. Attached Figure Description

[0052] Figure 1 This is a flowchart illustrating the method of an embodiment of this application;

[0053] Figure 2 This is a detailed schematic diagram of the method according to an embodiment of this application. Detailed Implementation

[0054] Reference Figure 1 and Figure 2 This application provides a method for online load identification and compensation of a robotic arm based on extended Kalman filtering, the method comprising:

[0055] S101, collects joint torque data of the robotic arm in multiple configurations under no-load conditions, and builds a no-load torque benchmark library;

[0056] S102, Collect the measured joint torque of the robotic arm when it is under load, and combine it with the no-load torque reference library to separate the pure load torque caused by the end load;

[0057] S103, based on extended Kalman filtering, takes the state vector containing load mass parameters and center of gravity offset parameters as the object to be identified, and the pure load torque as the observed variable. Through iterative calculations of prediction step and update step, the current load mass and center of gravity offset parameters are identified online.

[0058] S104, using the identified load mass and center of gravity offset parameters as dynamic variables, to make real-time corrections to the kinematic and dynamic models of the robotic arm;

[0059] S105 performs torque compensation control on the robotic arm based on the modified kinematic and dynamic models.

[0060] The measured joint torque of a robotic arm under load consists of three parts: the arm's own weight torque, the joint frictional resistance torque, and the eccentric torque generated by the end-effector load. The arm's own weight torque and the joint frictional resistance torque are only related to the robotic arm's structure and joint motion state, and are independent of whether the end-effector is under load. To obtain the pure load torque caused solely by load eccentricity, a pre-built unloaded torque benchmark library must be used to eliminate the unloaded component from the measured torque.

[0061] In step S101, under the unloaded state where the end of the robotic arm is not holding a workpiece, the position and posture are kept constant with the center of the end flange of the robotic arm as a reference. Through coordinated joint movements, no displacement or posture change at the end is achieved, thus acquiring data from the robotic arm under near-ideal, interference-free conditions. No-load torque data for joints in different configurations.

[0062] Furthermore, after averaging and filtering the original no-load torque data from multiple frames to reduce noise, a no-load torque benchmark library is constructed to provide a reliable benchmark reference for the effective separation of load torques in the future.

[0063] In step S102, each joint of the robotic arm has a built-in torque sensor to collect the joint output torque signal in real time. During the robotic arm's load-bearing operation, due to factors such as electromagnetic interference and mechanical vibration in the on-site environment, noise fluctuations inevitably get mixed into the original torque acquisition signal. Filtering and noise reduction and outlier removal are required to obtain a stable and reliable measured torque signal.

[0064] A sliding window averaging filter algorithm is used to smooth the raw torque data. Let the raw torque value of the i-th joint at time t be... If the sliding window length N is 10-15 sampling points (N is 12 in this embodiment), then the filtered smoothing torque value... The calculation formula is:

[0065]

[0066] in, For the index of dynamic sampling time within the window, from (Window start time) to (At the current moment). Filtering operations only occur at this time. The filtering algorithm takes effect only after the window has stored N frames of data, meaning a valid filtering result can only be output. This filtering algorithm can effectively suppress high-frequency noise and random pulse interference, improving the stability of torque signals.

[0067] After filtering, in order to further eliminate abnormal jump data caused by occasional sensor failures or strong electromagnetic shocks, this embodiment designs an outlier detection and correction mechanism based on the comparison of the average values ​​of adjacent frames.

[0068] The specific steps are as follows: For each frame of filtered torque value... Calculate its relationship with the five adjacent frames (i.e. , At times t+1 and t+2, if the current frame is acquired in real time, only the deviation of the mean torque from historical frames is referenced. When the absolute value of the deviation exceeds a preset threshold... If the data in a frame is deemed to be an outlier, the average of the next 5 frames is used to replace and correct the data in that frame. If the deviation does not exceed the threshold, the original filtered value is retained.

[0069] Let the average value of adjacent frames be The outlier detection and correction logic is as follows:

[0070]

[0071] After the above filtering, noise reduction, and outlier removal, a stable and reliable measured load torque value at time t of the i-th joint is obtained. This is used in the subsequent pure load torque separation step.

[0072] The measured joint torque of a robotic arm under load consists of three parts: the arm's own weight torque, the joint frictional resistance torque, and the eccentric torque generated by the end-effector load. The arm's own weight torque and the joint frictional resistance torque are only related to the robotic arm's structure and joint motion state, and are independent of whether the end-effector is under load. To obtain the pure load torque caused solely by load eccentricity, a pre-built unloaded torque benchmark library must be used to eliminate the unloaded component from the measured torque.

[0073] During loaded operations, the joint angle pose of the robotic arm is usually not completely consistent with the discrete pose points collected during no-load calibration. To achieve accurate torque separation, it is necessary to interpolate the no-load torque reference library based on the current joint attitude angle, so that the interpolated no-load torque is completely aligned with the pose information of the current loaded torque, thus eliminating torque separation errors caused by attitude differences.

[0074] Let the joint rotation vector at the current time t be... Search for the relevant value in the no-load torque reference library. For the nearest discrete pose points, the estimated value of the unloaded moment under the current pose is calculated using multivariate linear interpolation or radial basis function interpolation methods, denoted as . :

[0075]

[0076] in, is the interpolation function, and Library is the unloaded torque reference library.

[0077] The measured torque under load after filtering, noise reduction, and outlier removal is denoted as... The interpolated estimated no-load moment value By subtraction, we obtain the pure load torque caused solely by load eccentricity. :

[0078]

[0079] This difference calculation effectively eliminates the influence of the robot arm's own weight torque and joint frictional resistance torque, making... It can accurately reflect the additional torque information generated by the eccentricity of the end load, providing high-quality observation data for subsequent extended Kalman filter identification.

[0080] Taking a six-axis collaborative robotic arm as an example, the joint rotation angle at a certain sampling moment is... First, find the nearest pose points from the no-load torque reference library. and The corresponding no-load torques are respectively and The estimated unloaded moment under the current pose is calculated using linear interpolation. If the current measured torque under load is The pure load torque obtained by separation is:

[0081]

[0082] In this embodiment, the Extended Kalman Filter (EKF) algorithm is used. The mass parameters and center of gravity offset parameters of the end-effector load are used as the state variables to be identified, and the separated pure load torque is used as the observation variable. Through iterative recursive calculations of prediction steps and update steps, online real-time identification is achieved.

[0083] Step S103 of this application specifically includes:

[0084] Define a four-dimensional load eccentricity state vector It includes the load mass and the three-dimensional center of gravity offset moment, and is expressed as:

[0085]

[0086] Where: m is the load mass (unit: kg); Δx, Δy, Δz represent the three-dimensional offset coordinates of the load center of gravity relative to the center of the end flange (unit: m); radial eccentricity. .

[0087] Assuming the load parameters remain constant over a short period of time, the state transition model adopts a constant-value model, and the state equation is:

[0088]

[0089] Where X(t) is the true state value at time t. for The true state at time step. w(t) represents the process noise, and its covariance matrix is ​​Q, which is used to quantify the statistical characteristics of uncertainties such as modeling bias and small attitude perturbations.

[0090] Define a column vector of pure load torques corresponding to the number of joint modules in a robotic arm (taking a six-axis robotic arm as an example), with the following expression:

[0091]

[0092] in, The pure load torque obtained by separating the i-th joint at time t.

[0093] The relationship between the pure load torque and the load eccentricity state vector is nonlinear, and the nonlinear observation equation is established as follows:

[0094]

[0095] θ(t) is a nonlinear observation function that describes the mapping relationship between load eccentricity parameters, joint rotation angle, and pure load torque; θ(t) is the joint rotation angle vector at time t; v(t) is the observation noise, and its covariance matrix is ​​R, which is used to characterize the measured data errors such as sensor acquisition noise and signal transmission interference.

[0096] In this embodiment, a control and filtering cycle of 10ms is used, and a complete EKF iteration is performed in each cycle. The prediction step completes the prior estimation of the current time based on the best result of the previous time step.

[0097] Based on the optimal eccentricity parameters obtained from the previous iteration convergence, the predicted value of the state vector at the current moment is calculated. If the extended Kalman filter is in the initial startup phase, the initial eccentricity parameters are directly retrieved from the workpiece parameter reference library (the library contains commonly used basic workpiece parameters, which can effectively shorten the initial iteration convergence time). The formula for expressing the prior estimate of the covariance is:

[0098]

[0099] in, Let be the prior estimate of the state at time t. for The posterior optimal estimate of the state at time step 1.

[0100] The formula for the prior estimate of covariance is:

[0101]

[0102] in, Let be the prior estimate of the covariance at time t. for The time-wise covariance posterior estimate is given by Q, where Q is the process noise covariance matrix.

[0103] Based on prior state estimates and a nonlinear observation model, the theoretical pure load torque is calculated and used as a benchmark for error comparison with the measured pure load torque.

[0104]

[0105] in, The theoretically predicted torque at time t is generated by an idealized mathematical mapping that does not contain physical interference terms.

[0106] For nonlinear observation functions In the prior state estimate Performing a first-order Taylor expansion, we obtain the observation Jacobian matrix. To achieve linearization of the nonlinear observation model:

[0107]

[0108] This matrix establishes a linearized mapping relationship between state variables and observations, providing necessary support for subsequent core iterative operations such as Kalman gain calculation and state covariance matrix update.

[0109] The Kalman gain is calculated using the EKF standard formula and used to dynamically balance the confidence weights between model predictions and observed data.

[0110]

[0111] in, For Kalman gain, This is the observation noise covariance matrix. When the observation noise is low, the gain increases the weight of the observation data; when the model bias is low, the gain increases the weight of the model predictions to ensure the accuracy of the estimation results.

[0112] The difference between the measured pure load torque and the theoretically predicted torque is calculated to reflect the deviation between the model prediction and the actual operating conditions.

[0113]

[0114] in, This represents the torque residual.

[0115] To ensure reliable iteration of the EKF filtering algorithm, the measured pure load torque under the same operating condition must be satisfied. Compared with the theoretically predicted torque The results are largely consistent. Due to the inherent acquisition characteristics of the torque sensor and interference from the field environment, the measured torque inevitably contains observation noise. This embodiment sets a deviation threshold. Define the deviation value (i.e., the maximum absolute value of the torque residuals of each joint). When the deviation value is continuously greater than 3 frames (i.e., within 30ms) for 3 consecutive frames. If this is detected as model bias, EKF updates are paused and a nonlinear observation function is initiated. The parameters are fine-tuned; when the deviation value exceeds the limit in a single frame or discontinuous single frame, it is judged as observation spike noise, and the EKF filter is calculated normally in iterative calculation, thereby avoiding filter divergence caused by random noise.

[0116] Based on the torque residual and Kalman gain, the prior state estimate is corrected to obtain the most accurate estimate of the eccentricity parameter at the current time t:

[0117]

[0118] Update the posterior estimate of covariance:

[0119]

[0120] Where I is the identity matrix and P(t|t) is the posterior estimate of the covariance at time t.

[0121] After all update steps at time t are completed, the system automatically jumps back to the prediction step and displays the posterior state estimate obtained in this round. As the prior estimate of the state at the next time t+1 The covariance posterior value P(t|t) of this round is used as the input of the covariance prior estimate of the next time step. The theoretical predicted moment at time t+1 is recalculated, and a new round of prediction step-update step iterative loop is entered to realize the real-time update of the eccentricity parameter.

[0122] Simultaneously calculate the difference vector between the posterior and prior estimates: When the difference vector norm Less than the preset threshold If this state is maintained for 5 consecutive frames, it indicates that the EKF filter has converged and outputs stable eccentricity parameters. At the same time, switch to lightweight iteration mode (adjust the filtering period to 50ms) to reduce the iteration frequency and reduce computing power consumption; if the difference vector norm is greater than the preset threshold, immediately restore to the normal iteration level (10ms period) to ensure the real-time performance and accuracy of eccentric parameter identification.

[0123] Taking a six-axis collaborative robotic arm grasping an irregularly shaped workpiece as an example, the initial state is unknown. The initial estimated value (unit: kg, m) is retrieved from the workpiece parameter reference library:

[0124]

[0125] That is, the estimated mass m = 0.5 kg, and the center of gravity coincides with the center of the flange. ).

[0126] Let the process noise covariance matrix Q and the observation noise covariance matrix R be respectively:

[0127]

[0128] In the first filtering cycle (t=10ms), the prediction step is executed, and the prior state estimate is:

[0129]

[0130] The prior estimate of the covariance is:

[0131]

[0132] The theoretically predicted torque is calculated based on prior state estimates and a nonlinear observation model.

[0133]

[0134] The measured pure load torque is:

[0135]

[0136] Calculate the torque residual:

[0137] Calculate the observation Jacobian matrix:

[0138] Calculate the Kalman gain:

[0139] After performing the update step, the posterior state estimate is:

[0140]

[0141] Posterior update of covariance:

[0142] Repeat the above prediction and update steps, assuming the first step is... filter cycles ( The posterior estimate of the state is The prior state estimate is Define the difference vector:

[0143]

[0144] Calculate the norm of the difference vector:

[0145] After approximately 50 cycles (t=0.5s):

[0146] If this state is maintained for 5 consecutive frames (5 × 10 ms = 50 ms), the EKF is considered to have converged, and the final recognition result is output:

[0147]

[0148] Right now:

[0149] Calculate radial eccentricity:

[0150]

[0151] Since e≥5mm, the system enters the large eccentricity compensation mode.

[0152] In this embodiment, the load mass m and center of gravity offset parameters Δx, Δy, Δz obtained by EKF online identification are used as dynamic variables and integrated into the kinematic and dynamic models of the robotic arm in real time. This enables model migration from the flange center reference to the actual load center of gravity reference, fundamentally solving the problem of the disconnect between traditional fixed parameter models and actual working conditions.

[0153] Traditional robotic arm kinematic models use the center of the end effector flange as a reference point, assuming the load center of gravity coincides with the flange center. When the load is eccentric, this model cannot accurately describe the kinematic characteristics of the actual center of gravity at the end effector. This embodiment incorporates the identified center of gravity offset parameters into the kinematic model, shifting the reference point of the end effector from the robotic arm flange center to the actual load center of gravity.

[0154] Let the position vector of the end flange center of the robotic arm relative to the base coordinate system be... The attitude rotation matrix is The identified offset vector of the load center of gravity relative to the flange center is:

[0155]

[0156] The position vector of the actual load center of gravity in the base coordinate system is:

[0157] in, The three-dimensional coordinates of the load's center of gravity in the base coordinate system.

[0158] Regarding the joint rotation vectors on both sides above By taking the derivative, the mapping relationship between the joint angular velocity and the actual linear velocity of the end-effector can be derived:

[0159]

[0160] Since ΔP is considered constant after convergence ( ),and (in (where the antisymmetric matrix is ​​the terminal angular velocity) The above equation can be simplified to:

[0161]

[0162] in, This is the Jacobian matrix of the linear velocity with the flange center as the reference point in the traditional kinematic model.

[0163] By integrating the above mapping relationships, an eccentrically corrected Jacobian matrix that is adaptive to the current eccentricity condition is constructed. :

[0164]

[0165] in, It is a traditional six-dimensional Jacobian matrix (including linear velocity and angular velocity). Additional Jacobi correction term introduced for center of gravity shift.

[0166] EKF updates the state posterior estimate after each iteration (i.e., every 10ms). The system synchronously recalculates the eccentrically corrected Jacobian matrix. Replacing the original fixed Jacobian matrix This ensures that the mapping relationship between the end pose and joint rotation angle in the kinematic model always matches the actual load state.

[0167] The specific update formula is as follows:

[0168]

[0169] in, The centroid offset parameter is obtained at time t.

[0170] Through the above dynamic correction, the kinematic model can accurately reflect the position, velocity and acceleration of the actual center of gravity at the end, eliminating the positioning error caused by the center of gravity offset from the root.

[0171] Under normal centering load conditions, the center of gravity is assumed to coincide with the flange center, and the inertia tensor uses a fixed constant. Under eccentric load conditions, the center of gravity offset will change the load rotational inertia distribution, and the load inertia tensor needs to be recalculated based on the identified center of gravity offset parameters.

[0172] Let the inertia tensor of the load in its own center-of-mass coordinate system be... (This can be estimated in advance using the load geometry model or set as a default value), then the load's inertia tensor relative to the flange center... This can be calculated using the parallel axis theorem:

[0173]

[0174] m represents the identified load quality; I3 is the centroid offset vector; I3 is a 3×3 identity matrix; matrix This is the inertia tensor correction term in the parallel axis theorem.

[0175] The calculated Substitute the load inertia term into the dynamic equation of the robotic arm to ensure that the description of load inertia in the dynamic equation is consistent with the actual load state.

[0176] The joint friction coefficient is related to the actual force on the joint caused by eccentric load: the larger the eccentric load and the more obvious the center of gravity shift, the greater the force deviation of the joint. This embodiment relies on the eccentric parameters identified by EKF to calculate the real-time force load and the amplitude of the eccentric additional torque of each joint, and adaptively adjusts the friction coefficient accordingly.

[0177] Assume that the friction model for joint i adopts the LuGre model or a simplified viscous + Coulomb friction model:

[0178] Coulomb friction coefficient; It is the coefficient of viscous friction; This is the friction correction term caused by load eccentricity.

[0179] Based on the identified mass m and the center of gravity offset ΔP, calculate the magnitude of the eccentric additional moment:

[0180] in, Let θ be the angle between the direction of the force on joint i and the direction of gravity, and g be the acceleration due to gravity.

[0181] according to The magnitude of the friction coefficient is piecewise linearly adjusted:

[0182]

[0183] in, , The reference friction coefficient is obtained from the no-load calibration, and α and β are empirical adjustment coefficients (taken as 0.01 and 0.005 respectively in this embodiment).

[0184] For slight eccentricity and light load conditions (e<2mm), make small adjustments (α and β multiplied by a coefficient of 0.3), and for large eccentricity and heavy load conditions (e≥5mm), implement precise corrections (α and β multiplied by a coefficient of 1.0).

[0185] KF updates the state posterior estimate after each iteration (i.e., every 10ms). The dynamic parameters are corrected synchronously, forming a closed-loop chain of eccentricity identification, parameter update, and torque compensation.

[0186]

[0187] Substituting the updated dynamic parameters into the inverse dynamic equation, the theoretical compensation torque for each joint is calculated:

[0188] in, The term is the torque term generated by the load's gravity. This is the friction torque term. This is the inertial torque term.

[0189] In this embodiment, based on the load mass and center of gravity offset parameters identified by EKF, after real-time correction of the kinematic and dynamic models, the corrected models are further used to perform torque compensation control on the robotic arm. The theoretical compensation torque is fed forward and superimposed on the output command of the joint servo controller to offset the additional torque generated by the load eccentricity, thereby achieving high-precision and high-stability dynamic control.

[0190] This embodiment employs a control architecture of feedforward compensation and feedback closed-loop. The feedforward compensation section calculates the theoretical compensation torque based on the modified dynamic model to counteract the additional dynamic torque caused by load eccentricity. The feedback closed-loop section uses traditional PD / PID control to compensate for residual errors and external disturbances. The total control torque output is:

[0191]

[0192] This is the final control torque command for the i-th joint. The torque output by a feedback control law (such as PD control); This is the feedforward compensation torque calculated based on the modified model.

[0193] Based on the modified kinematic and dynamic models, the feedforward compensation torque The calculation includes the following components: load gravity compensation term, load centrifugal force and Coriolis force compensation term, and friction force compensation term.

[0194] The joint torque generated by the load gravity is related to the load mass, center of gravity position, and the current pose of the robotic arm. Using the corrected eccentricity parameters, the static torque generated by the load gravity on each joint is calculated:

[0195]

[0196] Let be the position vector of the load's center of gravity in the base coordinate system. ; The partial derivative of the load center of gravity position with respect to the joint angle can be obtained from the eccentrically corrected Jacobian matrix. Extract the linear velocity portion.

[0197] Load eccentricity alters the system's inertial characteristics, generating additional inertial torques during the robotic arm's acceleration and deceleration. This is addressed using the corrected load inertia tensor. Calculate the moment of inertia:

[0198]

[0199] in, Let j be the j-th row of the eccentrically modified Jacobian matrix.

[0200] In high-speed motion scenarios, load eccentricity significantly affects the distribution of centrifugal and Coriolis forces. Based on the corrected dynamic parameters, the centrifugal and Coriolis force terms are calculated as follows:

[0201]

[0202] in, For the nonlinear terms that include centrifugal force and Coriolis force, the calculation is recalculated based on the corrected mass and center of gravity parameters.

[0203] Based on the adaptively adjusted friction coefficient and Calculate the frictional torque:

[0204]

[0205] Adding the above values ​​together, we obtain the total feedforward compensation torque for the i-th joint: .

[0206] To compensate for model errors and external disturbances, this embodiment superimposes feedback control on top of torque compensation. A PD control law is used, and the formula for calculating the feedback torque is as follows:

[0207]

[0208] These represent the desired joint angular position and angular velocity, respectively. These represent the actual joint angular position and angular velocity, respectively. , These are the PD control gain coefficients (determined through tuning).

[0209] The final command torque acting on joint i is:

[0210]

[0211] in, For positional error, This represents the speed error.

[0212] EKF updates the state posterior estimate after each iteration (i.e., every 10ms). This triggers the following synchronization operation: update the eccentrically corrected Jacobian matrix. Update the load inertia tensor Adaptive adjustment of joint friction coefficient , Recalculate the feedforward compensation torque. The updated version The output is superimposed on the joint servo controller in real time.

[0213] When a robotic arm grasps irregularly shaped workpieces or performs eccentric clamping, the mass distribution of the end-effector load, especially the offset of the center of gravity relative to the flange center, significantly alters the force characteristics of each joint. Traditional control methods either rely on offline calibration, which cannot meet the needs of rapid changeover in flexible production lines, or only compensate for the load mass while ignoring the center of gravity offset, leading to a continuous disconnect between the dynamic model and actual working conditions, decreased positioning accuracy, and even joint overload due to excessive additional torque.

[0214] To address the aforementioned issues, this embodiment first constructs an unloaded torque reference library. By collecting joint torque data from multiple configurations under unloaded conditions, a benchmark is provided for subsequent separation of load torques. During loaded operation, joint torque signals are acquired in real time and preprocessed using a sliding window averaging filter and outlier removal algorithm to effectively suppress noise introduced by electromagnetic interference and mechanical vibration in industrial settings. Furthermore, the unloaded torque reference library is interpolated and aligned using the current joint attitude angle, ensuring that the interpolated unloaded torque and the measured loaded torque are under the same orientation conditions. By subtracting these values, the pure load torque caused solely by load eccentricity can be accurately separated.

[0215] After obtaining the pure load torque, this application introduces an extended Kalman filter (EKF) algorithm to identify the load mass and center of gravity offset parameters online. Traditional parameter identification methods often ignore the distinction between observation noise and model bias, which can easily lead to divergence in industrial environments with high sensor noise. This application constructs a four-dimensional state vector from the load mass and three-dimensional center of gravity offset, using the pure load torque as the observed variable, and solves it iteratively through the prediction and update steps of the EKF. In the prediction step, the prior estimate of the current state and the theoretically predicted torque are calculated based on the optimal estimate from the previous time step. In the update step, the prior estimate is corrected using the residual between the measured torque and the theoretically predicted torque, and the Kalman gain, to obtain the posterior optimal estimate. In particular, this application designs a bias threshold judgment mechanism. When the torque residual exceeds the threshold for multiple consecutive frames, it is judged as a model bias, triggering fine-tuning of the observation function parameters; while single-frame or discontinuous exceedances are judged as observation noise, and the EKF iterates normally. This mechanism can effectively distinguish between noise interference and actual model changes, avoiding filter divergence. Meanwhile, the preset workpiece parameter benchmark library provides initial values ​​for the first startup, shortening the convergence time; after convergence, switching to lightweight iterative mode reduces the computing power consumption of the embedded system.

[0216] The identified load mass and center of gravity offset parameters are not output in isolation, but are fed back as dynamic variables in real time to the kinematic and dynamic models for correction. This is the core of the closed-loop chain in this application. At the kinematic level, traditional models use the flange center as the reference point, and cannot accurately describe the motion of the actual center of gravity at the end when the center of gravity shifts. Based on the DH parameters and the identified center of gravity offset, this application constructs an eccentric correction Jacobian matrix, shifting the end reference point from the flange center to the actual load center of gravity, thereby eliminating the positioning error caused by center of gravity offset at its source. At the dynamic level, center of gravity offset changes the rotational inertia distribution of the load. Based on the identified mass and offset parameters, this application recalculates the load inertia tensor using the parallel axis theorem, replacing the fixed constant in the traditional scheme. Simultaneously, based on the eccentric parameters, the real-time force load and eccentric additional torque amplitude of each joint are calculated, and the joint friction coefficient is adaptively adjusted, with small adjustments for micro-eccentric conditions and precise corrections for large eccentric conditions. This dynamic parameter update mechanism ensures that the dynamic model adapts to the real-time load state throughout the process, solving the fundamental problem of the disconnect between the fixed parameter model and the actual working conditions.

[0217] Based on the modified kinematic and dynamic model, this application further implements torque compensation control. The feedforward compensation torque includes load gravity terms, inertial force terms, centrifugal force and Coriolis force terms, and friction force terms. These terms are calculated based on the real-time updated eccentricity-corrected Jacobian matrix, dynamic inertia tensor, and adaptive friction coefficient. The feedforward compensation torque and the feedback PD control torque are superimposed and applied to the joint servo controller to form a complete control command. Since the feedforward term has already offset most of the additional torque caused by load eccentricity, the feedback control only needs to compensate for residual errors and external disturbances, thereby significantly improving trajectory tracking accuracy and dynamic response performance.

[0218] To further optimize the balance between compensation effect and hardware protection, this application designs a graded adaptive compensation mechanism based on radial eccentricity. Radial eccentricity is calculated from the identified three-dimensional center of gravity offset parameters, and the system automatically classifies the compensation level based on this value. When the eccentricity is less than 2mm, it is considered a micro-eccentricity condition, where only a small correction is made to the Jacobian matrix and dynamic parameters, focusing on steady-state positioning compensation without reducing movement speed to ensure operational efficiency. When the eccentricity is between 2mm and 5mm, it is considered a small eccentricity condition, where the kinematic and dynamic models are fully and synchronously corrected and torque compensation is superimposed, balancing accuracy, trajectory stability, and operational efficiency, adapting to most common eccentricity conditions. When the eccentricity is greater than or equal to 5mm, it is considered a large eccentricity condition, where the upper speed limit is moderately reduced to 70% to 80% of the rated speed based on full compensation, and the acceleration and deceleration curves are optimized to avoid joint overload. This graded strategy achieves a precise match between compensation intensity and eccentricity degree, ensuring operational efficiency under small eccentricity conditions while protecting the robotic arm hardware under large eccentricity conditions.

[0219] In summary, this application establishes a complete technical chain encompassing no-load benchmark calibration, pure load torque separation, online EKF identification, dynamic correction of kinematic and dynamic models, torque compensation control, and graded adaptive compensation, forming a closed-loop link for eccentricity identification, parameter updating, and torque compensation. It achieves real-time and accurate identification and compensation of load mass and center of gravity offset without offline calibration, external tooling, or increased hardware costs. This effectively solves the technical pain points of traditional solutions, such as inability to adapt to rapid changeovers in flexible production lines, neglect of center of gravity offset, inability to dynamically update fixed models, limited compensation effects, and easy dispersion of filtering in noisy environments. Ultimately, it achieves a dual improvement in the positioning accuracy and motion stability of the robotic arm under varying load and eccentricity conditions.

[0220] This invention also provides a robotic arm load online identification and compensation device based on extended Kalman filtering, the device comprising:

[0221] The module is used to collect joint torque data of the robotic arm in multiple configurations under no-load conditions and build a no-load torque benchmark library.

[0222] The separation module is used to collect the measured joint torques of the robotic arm when it is under load, and combine them with the unloaded torque reference library to separate the pure load torque caused by the end load.

[0223] The identification module is used to identify the current load mass and center of gravity offset parameters online based on extended Kalman filtering, using a state vector containing load mass parameters and center of gravity offset parameters as the object to be identified, and the pure load torque as the observed variable, through iterative calculations of prediction step and update step.

[0224] The correction module is used to use the identified load mass and center of gravity offset parameters as dynamic variables to correct the kinematic and dynamic models of the robotic arm in real time.

[0225] The compensation module is used to perform torque compensation control on the robotic arm based on the modified kinematic and dynamic models.

[0226] It should be understood that the application of this application is not limited to the examples above. Those skilled in the art can make improvements or modifications based on the above description, and all such improvements and modifications should fall within the protection scope of the appended claims. Those skilled in the art can understand that implementing all or part of the processes of the above embodiments and making equivalent changes according to the claims of this application still fall within the scope of this application.

Claims

1. A method for online load identification and compensation of a robotic arm based on extended Kalman filtering, characterized in that, The method includes: Collect joint torque data of the robotic arm in multiple configurations under no-load conditions, and build a no-load torque benchmark library; Collect the measured joint torques of the robotic arm during loaded operation, and combine them with the unloaded torque reference library to separate the pure load torque caused by the end load; Based on extended Kalman filtering, the state vector containing load mass parameters and center of gravity offset parameters is taken as the object to be identified, and the pure load torque is taken as the observed variable. Through iterative calculations of prediction step and update step, the current load mass and center of gravity offset parameters are identified online. The identified load mass and center of gravity offset parameters are used as dynamic variables to correct the kinematic and dynamic models of the robotic arm in real time. Based on the modified kinematic and dynamic models, torque compensation control is performed on the robotic arm.

2. The method for online identification and compensation of robotic arm load based on extended Kalman filtering according to claim 1, characterized in that, The steps to separate the pure load torque caused by the end load include: The collected load torque data is averaged and filtered for noise reduction. Outlier removal is performed on the filtered load torque data; Interpolate the no-load torque reference library based on the current joint attitude angle to obtain the no-load torque that is aligned with the loaded torque pose; The pure load torque is obtained by subtracting the processed loaded torque from the interpolated unloaded torque.

3. The method for online identification and compensation of robotic arm load based on extended Kalman filtering according to claim 1, characterized in that, The steps for identifying the current load quality and center of gravity offset parameters online through iterative calculations of the prediction and update steps include: Establish the load eccentricity state vector; In the prediction step, the state prior estimate for the current time is calculated based on the state posterior estimate identified at the previous time step; the covariance prior estimate for the current time step is calculated based on the covariance posterior estimate and the process noise covariance matrix at the previous time step; and the theoretical predicted torque is calculated based on the state prior estimate and the nonlinear observation model at the current time step. In the update step, the torque residual is calculated based on the measured pure load torque and the theoretically predicted torque; a first-order Taylor expansion is performed on the nonlinear observation function at the state prior estimate to obtain the observation Jacobian matrix; the Kalman gain is calculated using the covariance prior estimate at the current time, the observation Jacobian matrix, and the observation noise covariance matrix; the state prior estimate at the current time is corrected using the torque residual and the Kalman gain to obtain the state posterior estimate at the current time; the covariance posterior estimate at the current time is updated using the Kalman gain, the observation Jacobian matrix, and the covariance prior estimate at the current time; the state posterior estimate is output as the identification result at the current time and input as the state posterior estimate for the next prediction step; the covariance posterior estimate is input as the covariance posterior estimate for the next prediction step. Calculate the Kalman gain, and use the torque residual and Kalman gain to correct the state prior estimate to obtain the state posterior estimate at the current time. The posterior state estimate is output as the identification result at the current time and used as the input for the prediction step at the next time.

4. The method for online identification and compensation of robotic arm load based on extended Kalman filtering according to claim 3, characterized in that, When the extended Kalman filter is first started, the initial eccentricity parameter is retrieved from the preset workpiece parameter reference library as the initial value for iteration.

5. The method for online identification and compensation of robotic arm load based on extended Kalman filtering according to claim 3, characterized in that, The method further includes: Set a deviation threshold and define the deviation value; When the torque residual is greater than the deviation threshold for multiple consecutive frames, it is determined to be a model deviation, the extended Kalman filter update is paused, and the parameter fine-tuning of the nonlinear observation model is initiated. When the torque residual is less than or equal to the deviation threshold for multiple consecutive frames, it is determined to be observation noise, and the extended Kalman filter iterates normally.

6. The method for online identification and compensation of robotic arm load based on extended Kalman filtering according to claim 3, characterized in that, The steps for real-time correction of the robotic arm's dynamic model include: Based on the DH parameters of the robotic arm and the identified center of gravity offset parameters, an eccentric correction Jacobian matrix that is adaptive to the current eccentric working condition is constructed to replace the original fixed Jacobian matrix, and the end reference point is moved from the flange center to the actual load center of gravity.

7. The method for online identification and compensation of robotic arm load based on extended Kalman filtering according to claim 1, characterized in that, The steps for real-time correction of the robotic arm's dynamic model include: The load inertia tensor is recalculated based on the identified center of gravity offset parameters to replace the fixed constant inertia tensor when the load center of gravity coincides with the flange center. Based on the identified eccentricity parameters, the real-time force load and eccentric additional torque amplitude of each joint are calculated, and the joint friction coefficient is adaptively adjusted accordingly.

8. The method for online identification and compensation of robotic arm load based on extended Kalman filtering according to claim 1, characterized in that, The method further includes: Calculate the radial eccentricity based on the identified load mass and center of gravity offset parameters; The torque compensation force and the upper limit of the movement speed are automatically adjusted according to the magnitude of the radial eccentricity.

9. The method according to claim 8, characterized in that, The automatic adjustment of torque compensation force and upper limit of movement speed based on the radial eccentricity includes: When the radial eccentricity is less than the first preset distance, a micro-eccentricity compensation mode with small-amplitude correction of kinematic and dynamic parameters is adopted. When the radial eccentricity is greater than or equal to the first preset distance and less than the second preset distance, a small eccentricity compensation mode with full synchronous correction and superimposed torque compensation is adopted. When the radial eccentricity is greater than or equal to the second preset distance, a large eccentricity compensation mode is adopted, which strengthens torque compensation and reduces the upper limit of the motion speed to a set ratio of the rated speed.

10. A robotic arm load online identification and compensation device based on extended Kalman filtering, characterized in that, The device includes: The module is used to collect joint torque data of the robotic arm in multiple configurations under no-load conditions and build a no-load torque benchmark library. The separation module is used to collect the measured joint torques of the robotic arm when it is under load, and combine them with the unloaded torque reference library to separate the pure load torque caused by the end load. The identification module is used to identify the current load mass and center of gravity offset parameters online based on extended Kalman filtering, using a state vector containing load mass parameters and center of gravity offset parameters as the object to be identified, and the pure load torque as the observed variable, through iterative calculations of prediction step and update step. The correction module is used to use the identified load mass and center of gravity offset parameters as dynamic variables to correct the kinematic and dynamic models of the robotic arm in real time. The compensation module is used to perform torque compensation control on the robotic arm based on the modified kinematic and dynamic models.

Images