An elastic joint torque estimation and control method based on extended Kalman filter

Abstract

This invention relates to the field of robotics, and more particularly to a method for estimating and controlling the torque of an elastic joint based on extended Kalman filtering. The method includes: Step 1, constructing a nonlinear dynamic model of the elastic joint based on a dual-mass-damped model, incorporating a current model and a spring measurement model, and obtaining the current-torque estimation information at the motor end of the elastic joint and the deformation-torque measurement results at the spring end; Step 2, testing the elastic joint using a dynamometer to identify the calibration parameters of the nonlinear dynamic model; Step 3, constructing a state-space expression for the elastic joint based on the identified nonlinear dynamic model, and designing an adaptive torque estimator and controller based on extended Kalman filtering, optimizing the output torque estimate and feeding it back to the controller, thereby adjusting the input current at the motor end and achieving closed-loop control of the joint's output torque. This invention can accurately estimate the dynamic output torque of the joint and use it as a feedback term to improve the force control accuracy of the joint.

Description

Technical Field

[0001] This invention relates to the field of robotics, and more particularly to a method for estimating and controlling the torque of an elastic joint based on extended Kalman filtering. Background Technology

[0002] The widespread application of modern robots in human-robot collaboration scenarios has placed higher demands on the safety, reliability, and compliance of human-robot interaction. Integrated joints with high-precision torque sensing and compliant control capabilities are gradually becoming a key hardware foundation for core robot drive components. Compared to traditional actuators, series elastic actuators achieve precise torque measurement by utilizing the elastic deformation of springs, possessing higher torque detection sensitivity and built-in buffering capabilities, thus showing broad application prospects in human-robot interaction tasks.

[0003] High-precision torque estimation is the core of achieving high-performance dynamic control of elastic joints. Currently, most systems employ dual or triple encoders to measure spring deformation in real time, thereby estimating external torque and generating the target output force. However, achieving high-precision torque sensing and high-response control still faces multiple challenges, including uncertain inertial forces of the actuator itself, nonlinear damping effects, and other unmodeled disturbances. Furthermore, unknown abrupt changes in the dynamic characteristics of the load end can easily introduce indistinguishable noise into transient torque measurement. Simultaneously, the bandwidth of torque measurement is significantly constrained by the elastic dynamics of the spring itself and the sensor signal processing frequency, which to some extent limits the application performance of elastic joints in practical human-machine interaction scenarios.

[0004] Therefore, accurately identifying the nonlinear dynamic parameters of the elastic integrated joint, calibrating and compensating for the error of the spring torque measurement model, and further designing an adaptive torque estimation algorithm and a high-precision robust controller have become key research directions for improving the torque control accuracy, and have important theoretical significance and engineering value. Summary of the Invention

[0005] To address the aforementioned technical problems in existing technologies, this invention proposes a method for estimating and controlling the torque of an elastic joint based on extended Kalman filtering. This method integrates current-torque estimation information from the motor end with deformation-torque measurement results from the spring end, and adaptively weights and fuses these two data using extended Kalman filtering to accurately estimate the dynamic output torque of the joint. This torque is then used as a feedback term to improve the force control accuracy of the joint. The specific technical solution is as follows:

[0006] A method for estimating and controlling the torque of an elastic joint based on extended Kalman filtering includes:

[0007] Step 1: Based on the dual-mass damping model, construct a nonlinear dynamic model of the elastic joint that includes a current model and a spring measurement model, and obtain the current-torque estimation information at the motor end of the elastic joint and the deformation-torque measurement results at the spring end.

[0008] Step 2: Use a dynamometer connected to the elastic joint to test it and identify the calibration parameters of the nonlinear dynamic model;

[0009] Step 3: Construct the state-space expression of the elastic joint based on the identified nonlinear dynamic model, and design an adaptive torque estimator and controller based on extended Kalman filter. Optimize the output torque estimate and feed it back to the controller to adjust the input current at the motor end, thereby realizing closed-loop control of the joint output torque.

[0010] Furthermore, in step one, the current model estimates the output torque of the elastic joint using the current, velocity, and angular velocity at the motor terminals, as expressed below:

[0011]

[0012] in , and These represent the motor end acceleration, motor end speed, and reduction ratio, respectively. Represents the motor's terminal inertia. Represents output torque; It is the current at the motor terminal. It is the torque-current coefficient at the motor end; The nonlinear damping term can be derived from the Stribeck friction model and the actual fitted curve:

[0013]

[0014] in , , and These represent static friction, Coulomb friction, Stribeck velocity, and empirical parameters, respectively.

[0015] Furthermore, in step one, the spring measurement model calculates its torque by measuring the input-output angle difference at the spring ends. Specifically, it measures the elastic deformation of the spring using dual encoders installed at the motor end and the load end, respectively. When the angle deviation between the spring's input and output ends... When the angle difference is small, the torque at the spring end is close to zero. When the angle difference is large, the torque at the spring end is linearly related to the angle difference, and its overall output torque... It can be calculated using the following formula:

[0016]

[0017] in and This is a constant value, representing the angle difference at the start of the linear phase of the spring; This represents the stiffness of the spring. There is a backlash when rotating forward and backward, so it needs to be calibrated separately.

[0018] Furthermore, in step two, the load is continuously increased using a dynamometer while maintaining a constant rotational speed, and the load-current curves at different speeds are linearly fitted to identify the torque-current coefficient and nonlinear friction term.

[0019] Furthermore, in step two, the encoder sampling data installed at the load end is subjected to bidirectional Gaussian filtering, then the encoder error is calibrated by piecewise linear compensation, and finally the residual error is calculated by an integral compensator to obtain the overall unmodeled error of the system.

[0020] Furthermore, in step two, the spring deformation under different loads is collected, the encoder data and the torque data obtained by the set torque sensor are resampled, and after Gaussian smoothing, the true torque at the output end is iterated based on the least squares method, and the spring stiffness in the forward and reverse directions is calibrated respectively.

[0021] Furthermore, in step three, the construction of the elastic joint state space expression specifically involves:

[0022] Establish the state-space equations for the elastic joint, where the estimated state value x includes the motor angle. Motor angular velocity , load angle Angular velocity of load With output torque :

[0023]

[0024] The motor terminal current i m external forces With the system input u set as the system input and the outputs being the motor angle, load angle, and output torque, the following state-space expression is constructed:

[0025]

[0026]

[0027]

[0028]

[0029] in Represents load-side damping. Represents the load-side inertia; This represents the stiffness of the spring; Represents the nonlinear damping term; This represents the motor's terminal inertia.

[0030] Furthermore, in step three, the extended Kalman filter is used to perform prior estimation and posterior correction of the elastic joint state x; wherein, the innovation matrix is ​​constructed. And calculate the new information covariance. This reflects the error between the model prediction value and the spring measurement torque at each moment; the process noise covariance matrix Q and the measurement noise covariance matrix R are set, with R being a fixed value and Q being adaptively and dynamically adjusted according to the information covariance matrix, so as to weight and fuse the current model estimate and the spring measurement model torque measurement value to obtain the optimized joint output torque estimate;

[0031] The optimized output torque estimate is used as a feedback signal and input to the feedback controller. By adjusting the input current at the motor end, closed-loop control of the joint output torque is achieved.

[0032] The beneficial effects of this invention include:

[0033] (1) By using the angle measurement information of the spring to reduce noise and accurately estimate the output torque, while maintaining the transient response accuracy of the current loop, the problem of low frequency response bandwidth of spring torque measurement is effectively solved.

[0034] (2) By accurately identifying nonlinear dynamic parameters, calibrating and compensating for spring torque measurement models, and combining adaptive torque estimation algorithms with high-precision robust controllers, the torque sensing accuracy and control performance of the elastic integrated joint are significantly improved, effectively overcoming the control bottleneck of traditional series elastic actuators under inertial force, nonlinear damping, unmodeled interference and dynamic changes at the load end. Attached Figure Description

[0035] Figure 1 This is a schematic diagram of a nonlinear dynamic model of an elastic integrated joint in this embodiment;

[0036] Figure 2 This is a schematic diagram of the elastic joint torque estimation and control process based on extended Kalman filtering in this embodiment.

[0037] Figure 3This is a comparison chart showing the effect of sinusoidal tracking of joint output torque using a conventional current loop PID controller and the control method of this invention. Detailed Implementation

[0038] To make the objectives, technical solutions, and technical effects of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and embodiments.

[0039] This embodiment of a method for estimating and controlling the torque of an elastic joint based on extended Kalman filtering includes:

[0040] Step 1, such as Figure 1 As shown, based on the dual-mass-damped model, a nonlinear dynamic model of the elastic joint, including a current model and a spring measurement model, is constructed to obtain the current-torque estimation information at the motor end of the elastic joint and the deformation-torque measurement results at the spring end.

[0041] The current model can estimate the torque at the output terminal by using the current, velocity, and angular velocity at the motor terminals, as expressed below:

[0042]

[0043] in , and These represent the motor end acceleration, motor end speed, and reduction ratio, respectively. Represents the motor's terminal inertia. Represents output torque; It's the current at the motor terminals, which can be measured in real time. It is the torque-current coefficient at the motor end. The nonlinear damping term can be derived from the Stribeck friction model and the actual fitted curve:

[0044]

[0045] in , , and These represent static friction, Coulomb friction, Stribeck velocity, and empirical parameters, respectively.

[0046] The output torque mentioned above is estimated by the current at the motor end. It has a high response frequency but a lot of noise, making it difficult to effectively track the torque.

[0047] The spring measurement model calculates the torque by measuring the input-output angle difference at the spring's ends. Specifically, the elastic deformation of the spring is measured using dual encoders installed at the motor and load ends, respectively. The torque is calculated based on the angle deviation between the spring's input and output ends. When the angle difference is small, the torque at the spring end is close to zero. When the angle difference is large, the torque at the spring end is linearly related to the angle difference, and its overall output torque... It can be calculated using the following formula:

[0048]

[0049] in and This is a constant value, representing the angle difference at the start of the linear phase of the spring; This represents the stiffness of the spring. There is a backlash when rotating forward and backward, so it needs to be calibrated separately.

[0050] The above output torque results are highly accurate but have a certain delay.

[0051] Step 2: Identify the nonlinear dynamic parameters.

[0052] The elastic joint was placed on a dynamometer for testing. While maintaining a constant speed, the load was continuously increased. The load-current curves at different speeds were first linearly fitted to identify the torque-current coefficient and the nonlinear friction term.

[0053] Due to factors such as installation clearance, encoder zero drift, and environmental noise, the joint's dynamic model contains unmodeled errors. These torque residual errors need to be compensated for, and then the spring stiffness is calibrated based on the encoder's angle measurements. (System unmodeled errors) It can be divided into the following parts:

[0054]

[0055] in This represents the measurement error of the encoder. This represents the steady-state residual error of the system.

[0056] To address the encoder's measurement error, the sampled data is first subjected to bidirectional Gaussian filtering. Then, the encoder error is calibrated through piecewise linear compensation. Finally, the residual error is calculated using an integral compensator to obtain the overall unmodeled error of the system.

[0057] To identify the spring stiffness, the spring deformation under different loads is first collected on a dynamometer. The data collected by the encoder and the torque data obtained by the set torque sensor are resampled and smoothed by Gaussian. The true torque at the output end can be iteratively obtained based on the least squares method, and the spring stiffness can be solved inversely.

[0058]

[0059]

[0060]

[0061]

[0062]

[0063]

[0064] In the formula, Let be the spring deformation (angle difference) at the i-th sampling point. This corresponds to the measured output torque value; and These represent the number of sampling points in the forward and reverse directions, respectively. The intercept term obtained from the linear fit is used to calculate... and .

[0065] The current model and spring measurement model described above were measured and calibrated on the dynamometer platform, and the results are shown in the table below:

[0066]

[0067] Step 3: Based on the identified nonlinear dynamic model, construct the state-space expression of the elastic joint and design an adaptive torque estimator and controller based on extended Kalman filtering. Optimize the output torque estimate and feed it back to the controller to adjust the input current at the motor end, thereby realizing closed-loop control of the joint output torque.

[0068] Establish the state-space equations for the elastic joint, where the estimated state value x includes the motor angle. Motor angular velocity , load angle Angular velocity of load With output torque :

[0069]

[0070] The motor terminal current i m external forces With the system input u set as the system input and the outputs being the motor angle, load angle, and output torque, the following state-space expression is constructed:

[0071]

[0072]

[0073]

[0074]

[0075] in Represents load-side damping. This represents the load-side inertia.

[0076] First, a priori estimation of the state variable x at time k is performed based on the discrete model:

[0077]

[0078]

[0079] in Represents the predicted value of the prior state. This represents the Jacobian matrix corresponding to the previous time step; This represents the updated covariance matrix, which reflects the uncertainty of the model's predictions. The process noise covariance matrix represents the level of confidence in the model's predictions, which can be adjusted by modifying the process covariance matrix.

[0080] Constructing the new information matrix And calculate the new information covariance. This reflects the error between the model's predicted value and the measured torque of the spring at each moment. Among them... Represents the actual measured value in equation (13) at time k. It is the prediction matrix of the discrete-time model, which maps the predicted value of the previous time step to the observation space; This is the measurement noise covariance matrix. Adjusting the measurement covariance matrix can adjust the level of confidence in the torque measurement values. The specific expression is as follows:

[0081]

[0082]

[0083] Calculate Kalman gain And the state value at the current moment Perform posterior estimation and update the covariance matrix. :

[0084]

[0085]

[0086]

[0087] The process noise covariance matrix Q and the measurement noise covariance matrix R are set as shown in equation (23). Since the measured value of the spring is relatively accurate, the measurement noise covariance matrix R remains constant, while the process noise covariance matrix Q is adaptively adjusted according to the new covariance matrix, as shown in equation (24). The weighting parameter α is 0.95. By setting the initial covariance matrix and adaptively adjusting it, the proportion of the spring torque measurement value and the current model estimate value can be adjusted, thereby improving the accuracy of the output torque estimation and effectively suppressing noise vibration.

[0088]

[0089]

[0090] The estimated torque inverse angle difference is used as feedback value and input to the feedback controller. High-precision torque control is achieved by adjusting the current, realizing closed-loop control of the output torque. The control diagram is shown below. Figure 2 As shown, it mainly includes an extended Kalman filter (EKF) torque estimator, a feedback controller, and a current (feedforward) controller. The control concept is clear and straightforward, and easy to implement.

[0091] like Figure 3 As shown, this control method performs sinusoidal tracking of the joint output torque, resulting in smaller torque fluctuations and higher accuracy compared to a conventional current loop PID controller.

[0092] In summary, the method of this invention integrates nonlinear dynamic system state estimation with real-time compensation for unknown disturbances. Through an adaptive weighted fusion strategy, it combines the high bandwidth characteristics of current estimation with the high precision advantage of spring deformation measurement, thereby achieving adaptive, high bandwidth torque control under model uncertainty and external variable loads.

[0093] The above description is merely a preferred embodiment of the present invention and is not intended to limit the invention in any way. Although the implementation process of the present invention has been described in detail above, those skilled in the art can still modify the technical solutions described in the foregoing examples or make equivalent substitutions for some of the technical features. All modifications and equivalent substitutions made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

Claims

1. A method for estimating and controlling the torque of an elastic joint based on extended Kalman filtering, characterized in that, include: Step 1: Based on the dual-mass damping model, construct a nonlinear dynamic model of the elastic joint that includes a current model and a spring measurement model, and obtain the current-torque estimation information at the motor end of the elastic joint and the deformation-torque measurement results at the spring end. Step 2: Use a dynamometer connected to the elastic joint to test it and identify the calibration parameters of the nonlinear dynamic model; Step 3: Construct the state-space expression of the elastic joint based on the identified nonlinear dynamic model, and design an adaptive torque estimator and controller based on extended Kalman filter. Optimize the output torque estimate and feed it back to the controller to adjust the input current at the motor end, thereby realizing closed-loop control of the joint output torque.

2. The method for estimating and controlling the torque of an elastic joint based on extended Kalman filtering as described in claim 1, characterized in that, In step one, the current model estimates the output torque of the elastic joint using the current, velocity, and angular velocity at the motor terminals, as expressed below: in , and These represent the motor end acceleration, motor end speed, and reduction ratio, respectively. Represents the motor's terminal inertia. Represents output torque; It is the current at the motor terminal. It is the torque-current coefficient at the motor end; The nonlinear damping term can be derived from the Stribeck friction model and the actual fitted curve: in , , and These represent static friction, Coulomb friction, Stribeck velocity, and empirical parameters, respectively.

3. The method for estimating and controlling the torque of an elastic joint based on extended Kalman filtering as described in claim 1, characterized in that, In step one, the spring measurement model calculates its torque by measuring the input-output angle difference at the spring ends. Specifically, it measures the elastic deformation of the spring using dual encoders installed at the motor end and the load end, respectively. The torque is calculated when the angle deviation between the spring's input and output ends... When the angle difference is small, the torque at the spring end is close to zero. When the angle difference is large, the torque at the spring end is linearly related to the angle difference, and its overall output torque... It can be calculated using the following formula: in and This is a constant value, representing the angle difference at the start of the linear phase of the spring; This represents the stiffness of the spring. There is a backlash when rotating forward and backward, so it needs to be calibrated separately.

4. The method for estimating and controlling the torque of an elastic joint based on extended Kalman filtering as described in claim 1, characterized in that, In step two, the load is continuously increased using a dynamometer while maintaining a constant speed. The load-current curves at different speeds are linearly fitted to identify the torque-current coefficient and nonlinear friction term.

5. The method for estimating and controlling the torque of an elastic joint based on extended Kalman filtering as described in claim 1, characterized in that, In step two, the encoder sampling data installed at the load end is subjected to bidirectional Gaussian filtering, then the encoder error is calibrated by piecewise linear compensation, and finally the residual error is calculated by an integral compensator to obtain the overall unmodeled error of the system.

6. The method for estimating and controlling the torque of an elastic joint based on extended Kalman filtering as described in claim 5, characterized in that, In step two, the spring deformation under different loads is collected, the encoder data and the torque data obtained by the set torque sensor are resampled, and after Gaussian smoothing, the true torque at the output end is iterated based on the least squares method, and the spring stiffness in the forward and reverse directions is calibrated respectively.

7. The method for estimating and controlling the torque of an elastic joint based on extended Kalman filtering as described in claim 1, characterized in that, In step three, the construction of the elastic joint state space expression is specifically as follows: Establish the state-space equations for the elastic joint, where the estimated state value x includes the motor angle. Motor angular velocity Load angle Angular velocity of load With output torque : The motor terminal current i m external forces With the system input u set as the system input and the outputs being the motor angle, load angle, and output torque, the following state-space expression is constructed: in Represents load-side damping. Represents the load-side inertia; This represents the stiffness of the spring; Represents the nonlinear damping term; This represents the motor's terminal inertia.

8. The method for estimating and controlling the torque of an elastic joint based on extended Kalman filtering as described in claim 7, characterized in that, In step three, the extended Kalman filter is used to perform prior estimation and posterior correction of the elastic joint state x; wherein, the innovation matrix is ​​constructed. And calculate the new information covariance. This reflects the error between the model prediction value and the spring measurement torque at each moment; the process noise covariance matrix Q and the measurement noise covariance matrix R are set, with R being a fixed value and Q being adaptively and dynamically adjusted according to the information covariance matrix, so as to weight and fuse the current model estimate and the spring measurement model torque measurement value to obtain the optimized joint output torque estimate; The optimized output torque estimate is used as a feedback signal and input to the feedback controller. By adjusting the input current at the motor end, closed-loop control of the joint output torque is achieved.

Images