Robot joint state estimation method and system based on adaptive Kalman filtering

Abstract

The invention relates to the field of robot intelligent control design, and discloses a robot joint state estimation method and system based on adaptive Kalman filtering, and the method comprises the following steps: S1, collecting the output data of a motor encoder of a robot joint and the torque feedback data of a joint driver; s2, establishing a joint state space model based on the dynamic relationship of the robot joints; s3, setting an observation vector according to the joint input data set; s4, setting initial values of a process noise covariance and an observation noise covariance, and constructing an improved adaptive Kalman filtering model; and S5, executing state prediction and state correction according to the improved adaptive Kalman filtering model. The technical effects of improving the robot joint trajectory tracking precision, reducing control jitter and enhancing the system stability are achieved by estimating the measurement noise and the process noise in real time and dynamically adjusting the filtering gain in cooperation with a feedback compensation strategy of a joint state estimation result in a control loop.

Description

Technical Field

[0001] This invention relates to the field of intelligent control design for robots, specifically to a method and system for estimating robot joint states based on adaptive Kalman filtering. Background Technology

[0002] Joint state estimation is a key technology in robot control systems. Real-time and accurate acquisition of joint position, velocity, acceleration, and other state variables provides fundamental support for motion control, force control, and safety protection functions. In recent years, Kalman filtering and its improved algorithms have been widely applied in joint state fusion, dynamic model compensation, and high-precision control scenarios due to their ability to provide relatively stable optimal estimates in noisy environments. Adaptive Kalman filtering, in particular, adjusts the noise covariance online, making it more flexible and adaptable than traditional filtering methods under complex conditions.

[0003] As robots develop towards higher speed, higher precision, and higher stability, joint sensors are susceptible to factors such as friction changes, load disturbances, and environmental noise fluctuations, leading to unstable measurement data. Meanwhile, dynamic models struggle to accurately describe the nonlinear characteristics of real-world systems, making it difficult for traditional filtering methods based on static noise assumptions to maintain consistent estimation accuracy. In related technologies, adaptive Kalman filtering generally focuses only on updating the statistical characteristics of noise using observation residuals. It is difficult to respond to the rapid changes in measurement noise and process noise in a timely and accurate manner. The filtering estimation results are usually only used as passive feedback input to the controller, lacking deep coupling with the control strategy. As a result, the system is still susceptible to noise mutations, model errors or external disturbances during trajectory tracking, which can lead to control jitter, increased tracking deviation and decreased system stability. Summary of the Invention

[0004] To address the shortcomings of existing technologies, this invention provides a robot joint state estimation method and system based on adaptive Kalman filtering. This solves the problem that the lack of deep coupling with the control strategy makes the robot susceptible to noise spikes, model errors, or external disturbances during trajectory tracking, leading to control jitter, increased tracking deviation, and decreased system stability.

[0005] To achieve the above objectives, the present invention provides the following technical solution: a robot joint state estimation method based on adaptive Kalman filtering, comprising the following steps: S1. Collect the output data of the motor encoder and the torque feedback data of the joint actuator of the robot joint, and perform time registration on the collected data to construct a joint input dataset; S2. Establish a joint state space model based on the dynamic relationship of robot joints. The joint state space model includes state variables such as joint angle, joint angular velocity, and joint acceleration. S3. Set the observation vector according to the joint input dataset, and construct the observation equation corresponding to the joint state space model; S4. Set the initial values ​​for the process noise covariance and the observation noise covariance, and construct an improved adaptive Kalman filter model for online adjustment of the noise covariance; S5. Perform state prediction and state correction based on the improved adaptive Kalman filter model, and calculate the estimated values ​​of joint angle, joint angular velocity and joint acceleration.

[0006] Preferably, step S1 includes the following steps: Perform a consistency check on the time series of motor encoder output data; Interpolation compensation is performed on the torque feedback data of the joint actuator based on the time difference of data acquisition. The timing of the data realignment operation is determined by setting a time registration threshold. This data realignment operation is used to improve the synchronization accuracy of the joint input dataset.

[0007] Preferably, step S2 includes the following steps: Set the inertia matrix based on the joint inertia parameters; Calculate the viscous damping term based on the joint damping characteristics; Calculate the friction torque term based on the joint friction characteristics; By setting dynamic equilibrium conditions, state transition equations are constructed in the joint state-space model to reflect the dynamic evolution of joint motion.

[0008] Preferably, step S3 includes the following steps: The joint angle is measured based on the output of the motor encoder. The equivalent acceleration observation is set based on the torque feedback of the joint actuator; The observation residual is constructed by calculating the deviation between the predicted observation and the actual observation, and the observation residual is used for subsequent noise covariance adjustment.

[0009] Preferably, step S4 includes the following steps: Calculate the statistical deviation between the predicted residuals and the observed residuals; The process noise adjustment factor is set based on the statistical deviation, and the adjusted process noise covariance is calculated. The observation noise adjustment factor is set based on the variance of the innovation sequence, and the adjusted observation noise covariance is calculated; Whether to trigger the noise covariance refresh operation is determined by judging the changing trend of the noise covariance.

[0010] Preferably, the state prediction in S5 includes: Calculate the predicted state value based on the state transition equation; Calculate the predicted covariance based on the process noise covariance; By determining whether the predicted covariance exceeds a preset stable range, a covariance constraint operation is triggered to maintain numerical stability during the prediction phase.

[0011] Preferably, the state correction in S5 includes: Calculate the Kalman gain based on the adjusted observation noise covariance; The state correction is calculated based on the Kalman gain and the observation residual. Adjust the estimated values ​​of joint angle, joint angular velocity, and joint acceleration based on the state correction amount; The determination of whether to trigger state constraint operations is made by setting physical feasibility conditions, in order to prevent the estimated value from exceeding the mechanical structure limit.

[0012] Preferably, the step S5 is followed by the following steps: Calculate the torque saturation of the joint actuator; Determine whether the driving force has a saturation trend based on the torque saturation value; When a saturation trend of the driving force is detected, a compensation factor is set and the estimated value is fed forward to improve the accuracy of the estimation results in saturation scenarios.

[0013] Preferably, step S5 includes the following steps: Set a state fluctuation threshold and calculate the fluctuation range of the estimated value; Determine whether the estimation process exhibits a divergence trend based on the fluctuation range; When a divergence trend occurs, the filter model is re-initialized to maintain the stability of the joint state estimation process.

[0014] A robot joint state estimation system based on adaptive Kalman filtering, the system comprising: The data acquisition module is used to acquire motor encoder output data and joint actuator torque feedback data and perform time registration; The state module is used to construct a joint state space model that includes joint angles, joint angular velocities, and joint accelerations. The filtering module is used to adjust the process noise covariance and observation noise covariance online based on the predicted residual and the observation residual, and to perform state prediction and state correction. The control module is used to perform constraint operations or compensation operations based on the torque saturation and physical feasibility conditions. The output module is used to output the final estimates of joint angles, joint angular velocities, and joint accelerations.

[0015] This invention provides a method and system for robot joint state estimation based on adaptive Kalman filtering. It has the following advantages: 1. This invention achieves the technical effects of improving robot joint trajectory tracking accuracy, reducing control jitter, and enhancing system stability by estimating measurement noise and process noise in real time and dynamically adjusting the filter gain, combined with a feedback compensation strategy for joint state estimation results in the control loop.

[0016] 2. This invention introduces an improved adaptive Kalman filter mechanism, which updates the process noise and observation noise covariance in real time based on the statistical deviation between the predicted residual and the observed residual, and triggers a covariance refresh operation when the noise trend is abnormal. This allows the filter to automatically adapt to complex dynamic environments such as load changes and frictional abrupt changes, effectively suppressing estimation divergence and improving the stability of the system under dynamic operating conditions.

[0017] 3. This invention explicitly introduces the inertia matrix, viscous damping term, and friction torque term into the state-space modeling, ensuring that the state evolution strictly follows the dynamic equilibrium laws of the robot joints, eliminating reliance on empirical models. This modeling approach gives the state prediction process clear physical meaning, effectively separates different disturbance sources, and improves the interpretability of the estimation results and the universality of the model. Attached Figure Description

[0018] Figure 1 This is a flowchart of the method of the present invention; Figure 2 This is a system architecture diagram of the present invention. Detailed Implementation

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

[0020] Example: Please see the appendix Figure 1 This invention provides a robot joint state estimation method based on adaptive Kalman filtering, comprising the following steps: S1. Collect the output data of the motor encoder and the torque feedback data of the joint actuator of the robot joint, and perform time registration on the collected data to construct a joint input dataset; S1 includes the following steps: Perform a consistency check on the time series of motor encoder output data; Interpolation compensation is performed on the torque feedback data of the joint actuator based on the time difference of data acquisition. The timing registration threshold is set to determine whether to trigger a data realignment operation, which is used to improve the synchronization accuracy of the joint input dataset.

[0021] Specifically, the raw angle sequence from the encoder is corrected into a continuous, non-abrupt angle curve that can be used for subsequent filtering, and the angle change and direction change are made in accordance with the joint motion characteristics. To this end, the system first calculates the time increment and angle increment: ; Then check if the angle is abnormal based on the preset threshold: ; The system further checks whether a non-physical change in direction has occurred: ; If the direction sign suddenly reverses and exceeds the threshold, then linear interpolation correction is performed: ; Obtain the continuous angle sequence And then expand it; in, Original angle value; Changes in adjacent angles; : Abnormal threshold for angle changes; : Symbol for the direction of angle change; : Threshold for determining directional jumps; The effective time points before and after interpolation correction; : Corrected angle value; After the above corrections, the torque and angle sequences are time-aligned, and time realignment is performed if the deviation is too large. Since the torque and angle come from different devices, their timestamps may differ; therefore, the torque values ​​need to be interpolated onto the angle's time axis. If the time deviation exceeds the synchronization threshold, additional time correction is required for realignment. The system first performs basic interpolation based on the angle timestamp: ; Next, the deviation between the angle timestamp and the torque timestamp is calculated: ; If the deviation exceeds the threshold, then a time correction is constructed and realigned: ; The realigned torque needs to be re-interpolated: ; Finally, a unified reference timeline was established: ; in: Original torque value; Torque obtained by angle-time interpolation; Timestamp discrepancy between the two types of sensors; Synchronization threshold; Time realignment correction amount; Correct timestamp; Ultimately, a unified timeline was established; After the above realignment, once a unified time axis is obtained, the noise needs to be dynamically estimated based on the local statistical characteristics of the angle and torque, and the confidence level of the sample needs to be generated for noise adjustment in adaptive Kalman filtering. First, the noise variance of the angle within the sliding window is calculated: ; Torque noise is smoothed using exponential weighting: ; Based on the above results, construct the observation noise matrix: ; Predicted residuals are used to update process noise: ; Process noise is updated using an exponential fusion method: ; Simultaneously calculate the overall confidence level of the current sample: ; Where: Local variance window length : Angle and torque noise variance Torque noise exponential smoothing coefficient Observation noise covariance : The observation vector consisting of angle and torque Observation matrix Predicted state Predicting residuals Process noise covariance Process noise update coefficient Kalman gain Current sample confidence level Interpolation span Reference torque noise; Anomalies and directional jumps are corrected, eliminating non-physical jumps caused by encoder jitter, zero-return jitter, or transient noise, making the angle trajectory suitable for integration into the state prediction model. The realignment mechanism based on time deviation detection can dynamically compensate for cross-device sampling drift, ensuring that the joint torque always maintains temporal consistency with the angle, thus improving the reliability of the observation vector.

[0022] S2. Establish a joint state space model based on the dynamic relationship of robot joints. The joint state space model includes state variables such as joint angle, joint angular velocity and joint acceleration. S2 includes the following steps: Set the inertia matrix based on the joint inertia parameters; Calculate the viscous damping term based on the joint damping characteristics; Calculate the friction torque term based on the joint friction characteristics; By setting dynamic equilibrium conditions, state transition equations are constructed in the joint state-space model to reflect the dynamic evolution of joint motion.

[0023] Specifically, an inertia matrix is ​​established based on the joint structural parameters, which is used for calculating the inertia terms in the subsequent dynamic equations. The system provides the joint inertia matrix based on the calibrated inertia constant or structural model: ; in, For joint angle; For the joint in state The inertia matrix is ​​given below, reflecting the inertial response of joint motion to torque. Then, based on the joint damping characteristics, a viscous damping term is calculated. To reflect the energy dissipation caused by joint motion velocity, a viscous damping term related to joint velocity is constructed. The viscous damping torque is expressed as: ; in: Joint angular velocity; The viscous damping coefficient is a scalar or attitude-dependent matrix; The viscous damping torque term characterizes the velocity-dependent damping effect. To reflect the frictional effect during low-speed or reversing joint movements, the system provides a frictional torque term, which mainly includes static friction and Coulomb friction components. The frictional torque can be expressed as: ; in: Coulomb friction coefficient; The friction coefficient is related to speed. Used to characterize the direction of friction; The frictional torque is used to compensate for frictional effects in the low-speed region. Based on the inertia matrix, damping term, and friction term, the system constructs the state transition equation of the joint state-space model according to the dynamic equilibrium conditions, which describes the dynamic evolution of joint motion. The dynamic equilibrium conditions are expressed as follows: ; Further constructing the state-space form, let the state vector be... ; The state transition equation is then: ; in: ; ; ; ; ; The state equation reflects the acceleration response and motion law of the joint under a given torque input; By setting the inertia matrix, calculating the viscous damping term, constructing the friction torque term, and modeling the state space under dynamic equilibrium conditions, the dynamic components such as inertia, damping, and friction are clearly separated, making the model structured and the parameters controllable. This enables the subsequent filtering algorithm to independently process disturbances from different physical sources. At the same time, the state transition equation is constructed based on physical laws, making the state evolution physically consistent. Compared with empirical models, this can significantly enhance the prediction accuracy and system interpretability.

[0024] S3. Set the observation vector based on the joint input dataset and construct the observation equation corresponding to the joint state space model; S3 includes the following steps: The joint angle is measured based on the output of the motor encoder. The equivalent acceleration observation is set based on the torque feedback of the joint actuator; The observation residuals are constructed by calculating the deviation between the predicted and actual observations, and these residuals are used for subsequent noise covariance adjustment.

[0025] Specifically, joint angles are extracted from the encoder output and used as direct observations in the state-space model. After undergoing consistency and continuity processing in S1, the angle signals provided by the encoder can be used as the system's angle observation inputs. The joint angle observations can be expressed as: ; in: Measurement of joint angles; The preprocessed continuous angle sequence is consistent with S1. Then, an equivalent acceleration observation is constructed using torque feedback to enhance the observation dimension, so that the state estimation not only depends on the angle but also reflects the dynamic response. Based on the dynamic equation, the actuator output torque is converted into equivalent acceleration. ; in: Equivalent acceleration observations; : The external input torque output by the driver; Joint inertia matrix; : Viscous damping term; : Friction torque term; The above equation represents the acceleration response of the system under the existing torque input conditions, converting the torque signal into an observable that can correspond to the state equation.

[0026] After obtaining the angle and equivalent acceleration, the system calculates the predicted observations based on the state prediction values ​​and compares them with the actual observations to construct the observation residuals. These residuals are used for subsequent dynamic adjustment of the noise covariance and are a key input for adaptive filtering. The expression for the observation residuals is: ; in: : No. The observation residual at any given moment; : Actual observed vector, derived from angular observations With acceleration observations composition; The observation matrix is ​​used to map the predicted state to the observation space; The predicted state is calculated based on the state transition equation; the above formula represents the difference between the actual measured quantity and the predicted quantity, and is used to determine the current noise level and the reliability of the observation.

[0027] By combining the encoder angle and torque equivalent acceleration, the observation dimension covers both position and dynamic response, improving the observability of state estimation. Furthermore, the torque signal is converted into acceleration observations through the dynamic equations, allowing the observed values ​​to directly participate in dynamic updates and enhancing physical consistency.

[0028] S4. Set the initial values ​​for the process noise covariance and the observation noise covariance, and construct an improved adaptive Kalman filter model for online adjustment of the noise covariance; S4 includes the following steps: Calculate the statistical deviation between the predicted residuals and the observed residuals; The process noise adjustment factor is set based on the statistical deviation, and the adjusted process noise covariance is calculated. The observation noise adjustment factor is set based on the variance of the innovation sequence, and the adjusted observation noise covariance is calculated; Whether to trigger the noise covariance refresh operation is determined by judging the changing trend of the noise covariance.

[0029] Specifically, this step compares the statistical consistency between the model's predicted residuals and the actual observed residuals to determine whether the current model's noise assumption still holds. The system processes the predicted observed residuals... Perform sliding statistical analysis to obtain its statistical bias: ; Among them: actual observation residuals (from S3); Theoretical prediction of residual covariance; Predicted state covariance; : Statistical deviation between predicted residuals and actual residuals, used to measure the degree of deviation of model noise.

[0030] The system utilizes statistical deviation. The process noise is adjusted so that its covariance updates dynamically with changes in system state. The process noise adjustment factor can be expressed as: ; The adjusted process noise covariance is updated as follows: ; Wherein: process noise adjustment factor; : The proportional coefficient used to control the adjustment range; The adjusted process noise covariance; the above formula realizes the dynamic increase or decrease of process noise based on model deviation, so as to keep the prediction model stable.

[0031] The observation residuals reflect the degree of uncertainty in the system's observations. The system constructs an observation noise adjustment factor by calculating the variance of the innovation sequence. ; The adjusted observation noise covariance is updated as follows: ; in: ; ; ; This update dynamically adjusts the observed noise model based on changes in the actual observed noise level. To prevent the continuous accumulation or abnormal divergence of noise covariance, the system incorporates a noise change trend judgment mechanism. When the process noise or observed noise covariance deviates from the reference range for an extended period, a noise covariance refresh operation is triggered, bringing the system back to a stable operating state. The trend judgment condition can be expressed as: ; If the following conditions are met: ; Then execute: ; in: ; ; The refresh operation is used to avoid the noise covariance from expanding infinitely or shrinking excessively, ensuring that the filter always remains numerically stable. This achieves a comprehensive effect of improving the reliability of input data, interpretable dynamic evolution, rich and consistent observation information, and dynamic updating of the filter noise model with the environment, thereby significantly enhancing the stability, robustness and real-time accuracy of the entire state estimation system.

[0032] S5. Perform state prediction and state correction based on the improved adaptive Kalman filter model, and calculate the estimated values ​​of joint angle, joint angular velocity and joint acceleration.

[0033] State prediction in S5 includes: Calculate the predicted state value based on the state transition equation; Calculate the predicted covariance based on the process noise covariance; By determining whether the predicted covariance exceeds a preset stable range, a covariance constraint operation is triggered to maintain numerical stability during the prediction phase.

[0034] Specifically, using the dynamic state-space model constructed with S2, the predicted state value for the next time step is calculated based on the state estimate obtained at the current time step. The state prediction adopts a discretized state transition form: ; in: : The predicted state value at time i; The optimal state estimate at the previous time step; : Input quantity (e.g., torque) at time i; : The state transition function obtained by discretizing the dynamic equation.

[0035] After obtaining the state prediction values, the system updates the prediction covariance matrix based on the process noise covariance to reflect the uncertainty of the state prediction: ; in: Predicting covariance The optimal covariance at the previous time step : Jacobian matrix of the state transition function with respect to the state The above formula describes the covariance propagation process during the prediction phase, which is derived from the process noise covariance of S4.

[0036] To prevent excessive increase or abnormal shrinkage of the prediction covariance from causing filtering instability, the system sets a covariance stability range. When the prediction covariance exceeds the allowable range, a covariance constraint operation is triggered to maintain numerical stability during the prediction phase. The judgment condition can be expressed as: ; Execute when conditions are met: ; in: Predicting the stable upper bound of covariance; Proj( ): Covariance-constrained projection operation, which re-constrains the matrix within the stable interval; this operation is used to avoid the Kalman gain from becoming abnormal or the system from diverging due to the infinite increase of the prediction covariance.

[0037] By calculating the predicted state value based on the dynamic model and obtaining the predicted covariance by combining the process noise propagation, and triggering the covariance constraint operation when the predicted covariance exceeds the stable range, this step realizes the numerical stability control of the prediction stage, so that the state prediction can maintain physical consistency and avoid covariance divergence, further improving the reliability and robustness of the entire adaptive filter under continuous operation.

[0038] State correction in S5 includes: Calculate the Kalman gain based on the adjusted observation noise covariance; The state correction is calculated based on the Kalman gain and the observation residual. Adjust the estimated values ​​of joint angle, joint angular velocity, and joint acceleration based on the state correction amount; The determination of whether to trigger state constraint operations is made by setting physical feasibility conditions, in order to prevent the estimated value from exceeding the mechanical structure limit.

[0039] Specifically, after completing the adaptive noise adjustment in S4, this step uses the updated observation noise covariance. Calculate the Kalman gain at the current time step to balance the confidence level between the predicted value and the actual observation: ; in: : No. Time-based Kalman gain; Predict covariance; Observation matrix; The adjusted observation noise covariance reflects the reliability of the actual measurement. The state correction is calculated based on the Kalman gain and observation residuals. The system utilizes the Kalman gain and observation residuals... The state correction is calculated jointly to correct for biases in the prediction results based on the observation information: ; in: : No. State correction amount at time; The estimated values ​​of joint angle, joint angular velocity, and joint acceleration are adjusted based on the state correction amount. A correction operation is then performed on the state vector based on the correction amount to ensure that the estimates of angle, angular velocity, and acceleration are optimally updated at the current moment. ; Wherein: the corrected state estimate includes , and ; Uncorrected prediction state; The correction amount is used to adjust the predicted state. To prevent the state estimate from exceeding the feasible range of the mechanical structure, the system sets physical constraints based on mechanical design parameters, including maximum joint angle, allowable speed, acceleration limits, etc. When the corrected state violates the physical feasibility conditions, the state constraint operation is triggered. ; in: Clamp The mechanism establishes physical upper and lower bounds for joint angles, angular velocities, and accelerations to prevent state out-of-bounds errors caused by numerical extrapolation and ensure the physical feasibility of the estimation. It calculates the Kalman gain using the adjusted observation noise covariance, generates state corrections based on observation residuals, performs optimal corrections on angles and velocities, and applies physical constraints to the estimation results after correction. This achieves effective fusion of observation information and physical credibility of state estimation, improving the accuracy of state updates and preventing estimated values ​​from exceeding structural limitations. Consequently, it further enhances the stability and reliability of the entire filtering system during dynamic operation.

[0040] S5 is followed by the following steps: Calculate the torque saturation of the joint actuator; Determine whether the driving force has a saturation trend based on the torque saturation value; When a saturation trend of the driving force is detected, a compensation factor is set and the estimated value is fed forward to improve the accuracy of the estimation results in saturation scenarios.

[0041] Specifically, to identify whether the driver is approaching its output limit, this step constructs a torque saturation value based on the difference between the current driver output torque and its rated maximum output torque, which is used to quantitatively characterize the degree of saturation approach. ; in: : No. The torque saturation at a given moment; : The maximum permissible output torque of the driver; Actual output torque; the smaller the saturation level, the closer it is to the saturation region. By setting a minimum acceptable saturation margin, when the saturation level is below the threshold, it indicates that the driver output is approaching its limit and tends to enter the saturation region. if The drive exhibits a saturation trend. in, The set torque saturation margin threshold is used to determine whether there is a saturation trend. When a saturation trend is detected, subsequent compensation operations will be triggered. When the system detects that the driver output is approaching the saturation region, this step constructs a feedforward compensation factor based on the saturation amount, so that the state estimation can reflect the estimation deviation caused by saturation nonlinearity in advance. The compensation factor can be constructed as follows: ; in, Torque saturation compensation factor; Compensation factor proportional coefficient; The numerator reflects the degree of saturation approximation; the closer to the saturation region, the greater the compensation. The compensation factor is used to perform feedforward correction for states that are significantly affected by torque, such as joint angular velocity and acceleration, thereby improving the estimation accuracy under saturation conditions. ; in: : The corrected state estimate (from S5 above); : Compensation mapping function related to torque direction or dynamic model, used to generate the deviation compensation amount caused by torque saturation; feedforward compensation enables state prediction to avoid estimation lag or deviation accumulation caused by torque clipping.

[0042] By calculating the torque saturation and identifying the saturation trend of the driving force, a compensation factor is constructed when the saturation region is detected, and feedforward compensation is performed on the state estimation. This can correct the estimation deviation caused by saturation nonlinearity in advance, so that the filter can still maintain estimation accuracy and response consistency under the condition of limited output of the driver. This significantly improves the robustness and reliability of the system in saturation scenarios such as high load and fast action.

[0043] S5 includes the following steps: Set a state fluctuation threshold and calculate the fluctuation range of the estimated value; Determine whether the estimation process exhibits a divergence trend based on the fluctuation range; When a divergence trend occurs, the filter model is re-initialized to maintain the stability of the joint state estimation process.

[0044] Specifically, to monitor whether excessive oscillations or numerical instability occur during the state estimation process, this step constructs a state fluctuation amplitude index based on the difference between the current corrected state estimate and the estimate at the previous time step: ; System set fluctuation threshold This is used to define the maximum permissible range of state fluctuations. Wherein: : No. The fluctuation range of the state estimate at any given time; : Optimal state estimation between two adjacent time points; : The maximum permissible fluctuation threshold defined by the system.

[0045] When the fluctuation range of the state estimate Continuously exceeding the threshold Or a sharp increase in a short period of time indicates that a potential numerical value has appeared in the filtering process. The divergence requires triggering subsequent stabilization mechanisms: if The system is determined to have a divergent trend. in: The criterion for judgment is whether the fluctuation range exceeds the threshold. Multiple consecutive exceedances can be used as a criterion to enhance reliability. When a divergence trend is detected in the state estimation, the system reinitializes the core variables of the filter to prevent further error accumulation, bringing the estimation process back to a controllable range. The reinitialization operation can be described as follows: ; in, : Initial state estimate or reference state reconstructed from the most recent stable moment; The preset initial covariance is used to reset the estimation uncertainty; reinitialization can prevent the filter divergence from propagating to subsequent time steps. By setting a state fluctuation threshold and calculating the estimated fluctuation amplitude, identifying the divergence trend of the filtering process based on the fluctuation characteristics, and re-initializing the filtering model in a timely manner when divergence signs are detected, real-time monitoring and recovery of the numerical stability of the state estimation are achieved. This enables the system to quickly return to a stable working state under abnormal disturbances, sensor mutations, or model mismatches, thereby further improving the reliability and long-term robustness of the joint state estimation process.

[0046] Please see the appendix Figure 2 A robot joint state estimation system based on adaptive Kalman filtering, the system includes: The data acquisition module is used to acquire motor encoder output data and joint actuator torque feedback data and perform time registration; The state module is used to construct a joint state space model that includes joint angles, joint angular velocities, and joint accelerations. The filtering module is used to adjust the process noise covariance and observation noise covariance online based on the predicted residual and the observation residual, and to perform state prediction and state correction. The control module is used to perform constraint operations or compensation operations based on the torque saturation and physical feasibility conditions. The output module is used to output the final estimates of joint angles, joint angular velocities, and joint accelerations.

[0047] Specifically, the data acquisition module collects joint angle data output from the motor encoder and torque feedback data from the joint actuator, and performs time registration to align multi-source data under the same time reference, ensuring the continuity and consistency of the input data. Furthermore, this module can use a unified timestamp and interpolation method to fill in missing data to meet real-time estimation requirements. The state module constructs a state-space model based on joint dynamics, including joint angles, angular velocities, and accelerations, ensuring that state evolution is consistent with the actual dynamics of the robot joints and improving the reliability of the estimation results. The filtering module adjusts the process noise covariance and observation noise covariance online based on the changing characteristics of the predicted and observed residuals to adapt to dynamic conditions such as load changes and acceleration. It also performs state prediction and correction to obtain the real-time optimal estimate of the joint state. When continuous abnormal state fluctuations are detected, the filtering module triggers a re-initialization operation to restore the most recent stable state, thus preventing estimation divergence. Based on the torque saturation characteristics of the joint actuator, the control module performs feedforward compensation when the driving force is detected to be approaching the saturation range. Furthermore, it applies physical constraints to the estimation results based on the joint travel range, maximum speed, and acceleration limits to prevent the estimated values ​​from exceeding the structural allowable range. The output module is used to output the final estimated results of joint angle, joint angular velocity and joint acceleration after completing prediction, correction, compensation and constraint processing, providing real-time state information for the robot control system. During system operation, it executes the process of data acquisition, state prediction, state correction, compensation and constraint and result output in a loop to achieve high-precision and reliable joint state estimation.

[0048] Although embodiments of the invention have been shown and described, it will be understood by those skilled in the art that various changes, modifications, substitutions and alterations can be made to these embodiments without departing from the principles and spirit of the invention, the scope of which is defined by the appended claims and their equivalents.

Claims

1. A robot joint state estimation method based on adaptive Kalman filtering, characterized in that, Includes the following steps: S1. Collect the output data of the motor encoder and the torque feedback data of the joint actuator of the robot joint, and perform time registration on the collected data to construct a joint input dataset; S2. Establish a joint state space model based on the dynamic relationship of robot joints. The joint state space model includes state variables such as joint angle, joint angular velocity, and joint acceleration. S3. Set the observation vector according to the joint input dataset, and construct the observation equation corresponding to the joint state space model; S4. Set the initial values ​​for the process noise covariance and the observation noise covariance, and construct an improved adaptive Kalman filter model for online adjustment of the noise covariance; S5. Perform state prediction and state correction based on the improved adaptive Kalman filter model, and calculate the estimated values ​​of joint angle, joint angular velocity and joint acceleration.

2. The robot joint state estimation method based on adaptive Kalman filtering according to claim 1, characterized in that: S1 includes the following steps: Perform a consistency check on the time series of motor encoder output data; Interpolation compensation is performed on the torque feedback data of the joint actuator based on the time difference of data acquisition. The timing of the data realignment operation is determined by setting a time registration threshold. This data realignment operation is used to improve the synchronization accuracy of the joint input dataset.

3. The robot joint state estimation method based on adaptive Kalman filtering according to claim 1, characterized in that: S2 includes the following steps: Set the inertia matrix based on the joint inertia parameters; Calculate the viscous damping term based on the joint damping characteristics; Calculate the friction torque term based on the joint friction characteristics; By setting dynamic equilibrium conditions, state transition equations are constructed in the joint state-space model to reflect the dynamic evolution of joint motion.

4. The robot joint state estimation method based on adaptive Kalman filtering according to claim 1, characterized in that: S3 includes the following steps: The joint angle is measured based on the output of the motor encoder. The equivalent acceleration observation is set based on the torque feedback of the joint actuator; The observation residual is constructed by calculating the deviation between the predicted observation and the actual observation, and the observation residual is used for subsequent noise covariance adjustment.

5. The robot joint state estimation method based on adaptive Kalman filtering according to claim 1, characterized in that: S4 includes the following steps: Calculate the statistical deviation between the predicted residuals and the observed residuals; The process noise adjustment factor is set based on the statistical deviation, and the adjusted process noise covariance is calculated. The observation noise adjustment factor is set based on the variance of the innovation sequence, and the adjusted observation noise covariance is calculated; Whether to trigger the noise covariance refresh operation is determined by judging the changing trend of the noise covariance.

6. The robot joint state estimation method based on adaptive Kalman filtering according to claim 1, characterized in that: The state prediction in S5 includes: Calculate the predicted state value based on the state transition equation; Calculate the predicted covariance based on the process noise covariance; By determining whether the predicted covariance exceeds a preset stable range, a covariance constraint operation is triggered to maintain numerical stability during the prediction phase.

7. The robot joint state estimation method based on adaptive Kalman filtering according to claim 1, characterized in that: The state correction in S5 includes: Calculate the Kalman gain based on the adjusted observation noise covariance; The state correction is calculated based on the Kalman gain and the observation residual. Adjust the estimated values ​​of joint angle, joint angular velocity, and joint acceleration based on the state correction amount; The determination of whether to trigger state constraint operations is made by setting physical feasibility conditions, in order to prevent the estimated value from exceeding the mechanical structure limit.

8. The robot joint state estimation method based on adaptive Kalman filtering according to claim 1, characterized in that: S5 includes the following steps: Set a state fluctuation threshold and calculate the fluctuation range of the estimated value; Determine whether the estimation process exhibits a divergence trend based on the fluctuation range; When a divergence trend occurs, the filter model is re-initialized to maintain the stability of the joint state estimation process.

9. The robot joint state estimation method based on adaptive Kalman filtering according to claim 1, characterized in that: The step following S5 is: Calculate the torque saturation of the joint actuator; Determine whether the driving force has a saturation trend based on the torque saturation value; When a saturation trend of the driving force is detected, a compensation factor is set and the estimated value is fed forward to improve the accuracy of the estimation results in saturation scenarios.

10. A robot joint state estimation system based on adaptive Kalman filtering, applied to the robot joint state estimation method based on adaptive Kalman filtering as described in any one of claims 1-9, characterized in that, The system includes: The data acquisition module is used to acquire motor encoder output data and joint actuator torque feedback data and perform time registration; The state module is used to construct a joint state space model that includes joint angles, joint angular velocities, and joint accelerations. The filtering module is used to adjust the process noise covariance and observation noise covariance online based on the predicted residual and the observation residual, and to perform state prediction and state correction. The control module is used to perform constraint operations or compensation operations based on the torque saturation and physical feasibility conditions. The output module is used to output the final estimates of joint angles, joint angular velocities, and joint accelerations.

Images