Joint moment-based humanoid machine action coordination method and system

Abstract

The invention provides a humanoid robot action coordination method and system based on joint torque, and the method comprises the steps: obtaining the original data of the joint torque of a humanoid robot through a sensor, carrying out the preprocessing of the original data of the torque, outputting stable torque data, constructing an impedance control model through the combination of the angle data of the mechanical arm joint of the humanoid robot, and carrying out the control of the mechanical arm joint. Predicting a moment change trend to obtain a predicted moment sequence; adaptive threshold detection is carried out on the predicted moment sequence, and moment peak values of different amplitudes are identified; analyzing the propagation velocity of the torque peak value, and calculating a velocity vector of peak value propagation by combining the propagation direction of the torque peak value; whether the velocity vector of peak propagation exceeds a preset threshold value or not is judged, and if yes, the response trend of a joint torque controller is predicted to judge whether joint control parameters of the humanoid robot need to be adjusted or not; and if the joint control parameters of the humanoid robot need to be adjusted, optimizing the rigidity and damping parameters of the impedance control model to output a stable control signal.

Description

Technical Field

[0001] The present invention relates to the field of information technology, and particularly to a humanoid robot motion coordination method and system based on joint torque. Background Art

[0002] Compliant interaction control of humanoid robots is a key direction in the field of robotics. Its importance lies in achieving the safety and efficiency of human-robot collaboration. Especially in industrial, medical, and service scenarios, robots need to have the ability to interact naturally with humans and at the same time respond to complex dynamics of unexpected contacts. Currently, compliant control methods based on torque sensing have improved the interaction performance to a certain extent, but existing solutions still have significant limitations. Many control strategies perform poorly in dealing with joint torque measurement noise, resulting in a decrease in impedance control accuracy. Especially in scenarios of unexpected contacts, it is difficult to accurately predict the propagation of torque peaks and the saturation response of the controller. Especially during unexpected contacts, the propagation characteristics of the contact point torque peak are complex, which easily triggers the saturation response of the joint torque controller, leading to control instability and making it difficult for the robot to achieve stable torque control and safe interaction responses in human-robot collaboration. Summary of the Invention

[0003] The present invention provides a humanoid robot motion coordination method based on joint torque, mainly including:

[0004] Obtain the original joint torque data of the humanoid robot through sensors. After denoising the torque data, calculate the frequency components of the torque signal, extract the noise residue features using Fourier transform, and determine whether the noise residue is lower than a preset threshold. If it is lower than the threshold, output the stable torque data;

[0005] For the stable torque data, construct an impedance control model, predict the torque change trend, and obtain a predicted torque sequence;

[0006] Through adaptive threshold detection of the predicted torque sequence in scenarios of unexpected contacts, identify torque peak points of different amplitudes;

[0007] Adopt a non-uniform sliding window method to analyze the propagation speed of torque peaks, dynamically adjust the window size according to the severity of torque changes, and at the same time assign different weights to peak points of different amplitudes, calculate the propagation speed, and use vector analysis methods to calculate the velocity vector of peak propagation in combination with the directionality of torque peak propagation;

[0008] Determine whether the peak propagation velocity vector exceeds a preset threshold. If it exceeds, then predict the response trend of the joint torque controller in combination with the magnitude and direction of the peak propagation speed and the peak amplitude change rate. If the predicted controller response leads to saturation, pre-adjust the stiffness and damping parameters in advance;

[0009] Optimize the stiffness and damping parameters of the impedance control model according to the peak torque propagation speed vector and the rate of change of amplitude, and obtain the optimized control parameters;

[0010] For the optimized control parameters, calculate the output response of the controller, and determine whether the amplitude of the controller output response is within the preset range. If it is within the preset range, output a stable control signal;

[0011] Obtain the elastic parameters of the mechanical structure, calculate the error influence of elastic deformation on torque measurement by using the finite element analysis method, obtain the error compensation coefficient, and correct the stable control signal through the error compensation coefficient to obtain the final control output.

[0012] Furthermore, obtain the original joint torque data of the humanoid robot through sensors. After denoising the torque data, calculate the frequency components of the torque signal, extract the noise residue characteristics by using Fourier transform, and determine whether the noise residue is lower than the preset threshold. If it is lower than the threshold, output the stable torque data, including: collect data of multiple joint points of the humanoid robot through the angular velocity sensor and the torque sensor, and obtain the original joint torque data and the original angular velocity data according to the preset sampling frequency. Segment the collected data according to the preset time window length to obtain segmented torque data and segmented angular velocity data. For the segmented angular velocity data, calculate the mean and standard deviation of the angular velocity of each joint point, and mark the torque data of the corresponding time period according to the comparison result between the angular velocity standard deviation and the preset threshold, and select the torque data corresponding to the stable time period with smaller angular velocity fluctuations. For the selected torque data, perform multi-scale analysis by using the three-layer wavelet transform decomposition method, calculate the relative energy value of each layer of decomposition coefficient, screen each layer of coefficient according to the set energy threshold, and use the selected wavelet coefficients for signal reconstruction to obtain the denoised torque data. Perform frequency domain transformation on the denoised torque data, calculate the spectrogram by using the fast Fourier transform, extract the frequency components with amplitudes greater than the preset threshold in the spectrogram, and statistically obtain the noise residue characteristic value of the energy ratio corresponding to these frequency components. If the noise residue characteristic value is less than the preset threshold, perform smoothing processing on the denoised torque data by using the moving average method, set the moving window length to one-thousandth of the number of sampling points, and calculate the mean value of the torque data within the window as the smoothing result. Calculate the rate of change of torque between adjacent sampling points for the smoothed torque data. If the rate of change of torque at a certain point exceeds the preset threshold, mark this point as an abnormal point, and correct the abnormal point by using the median of the three sampling points before and after this point to obtain the stable torque data.

[0013] Furthermore, for the stable torque data, an impedance control model is constructed to predict the torque change trend and obtain a predicted torque sequence, including: constructing an impedance control parameter matrix based on the stable torque data and the joint angle data of the robotic arm, calculating the mapping relationship between the joint position and the torque through the joint stiffness coefficient and damping coefficient, setting corresponding impedance parameters for multiple joints of the robotic arm to obtain an initial impedance parameter matrix. For the initial impedance parameter matrix, the unit step response method is used to verify the impedance characteristics of each joint, calculate the steady-state error and overshoot corresponding to the impedance parameters, and correct the impedance parameters according to the preset threshold to obtain a corrected impedance parameter matrix. Using the corrected impedance parameter matrix and real-time position feedback data, a motion state space equation of the robotic arm is established. Selecting the joint angle, angular velocity, and torque as state variables, an observation matrix is constructed to describe the relationship between the state variables and the measured values, and the process noise covariance and measurement noise covariance of the state equation are initialized. According to the state space equation, the Kalman filter algorithm is used to calculate the state estimate value, the residual between the predicted state and the actual observed value is calculated based on the observation matrix, and the state estimate value is corrected through the Kalman gain to obtain a corrected state estimate value. Using the corrected state estimate value for forward recursive prediction, the state prediction value at the future moment is calculated according to the preset prediction step length, and the torque component in the predicted state value is extracted to form a torque prediction sequence. A sliding window process is performed on the torque prediction sequence, and the mean square error between the prediction sequence and the actual torque sequence is calculated. If the mean square error exceeds the preset threshold, the noise covariance matrix in the state space equation is updated, and the state estimation and torque prediction are performed again.

[0014] Furthermore, by performing adaptive threshold detection on the predicted torque sequence in the accidental contact scenario, torque peak points of different amplitudes are identified, including: calculating the torque difference sequence of adjacent sampling points based on the predicted torque sequence, calculating the torque change rate within a window using a sliding time window with a fixed length of 50 sampling points, normalizing the torque change rate sequence using the maximum-minimum method to obtain a standardized torque change feature sequence. For the standardized torque change feature sequence, calculate the local mean and standard deviation using a time window with a fixed length of 100 sampling points, set the adaptive detection threshold as the local mean plus or minus three times the standard deviation, and generate the upper and lower dynamic threshold boundary curves. According to the dynamic threshold boundary curves, mark the data points in the torque change feature sequence that exceed the upper threshold boundary or are lower than the lower threshold boundary, segment the marked points using a time window of 500 milliseconds to obtain candidate torque peak intervals. For each candidate torque peak interval, use the peak detection method to calculate the maximum and minimum points within the interval, judge the validity of the peak according to the amplitude difference between adjacent extreme points, and extract the effective torque peak points and their corresponding timestamp information. For the extracted torque peak data, set three classification intervals according to the peak amplitude, use the k-means clustering method based on Euclidean distance to classify the peak points, calculate the clustering center of each category as the amplitude reference value of the peaks in this category. Classify the torque peak points according to the amplitude reference value of the clustering center, record the amplitude category label, specific torque value and corresponding timestamp information of each peak point, and generate a torque peak data set with classification annotations.

[0015] Furthermore, the non-uniform sliding window method is used to analyze the propagation speed of the torque peak. The window size is dynamically adjusted according to the severity of the torque change. At the same time, different weights are assigned to the peak points of different amplitudes, and the propagation speed is calculated. The vector analysis method is used to calculate the velocity vector of the peak propagation in combination with the directionality of the torque peak propagation, including: calculating the torque change rate sequence according to the difference between adjacent torque sampling points, setting the reference sliding window size to 100 times the sampling period, and using the gradient-based adaptive method to judge the torque change rate. If the change rate exceeds 20% of the peak amplitude, the window size is reduced to one-fourth of the reference window. If the change rate is lower than 5% of the peak amplitude, the reference window size remains unchanged. For the torque peak points of different amplitudes, the peak amplitudes are normalized according to the maximum peak amplitude, and the normalized amplitudes are mapped to the weight interval of 0.5 to 1.5 through a linear function. The normalized peak data and the weight coefficients are weighted and combined to obtain the weighted torque peak sequence. According to the physical installation positions of the sensors on multiple joints of the robotic arm, a three-dimensional rectangular coordinate system with the base of the robotic arm as the origin is established, and the position coordinates of each sensor are transformed into this coordinate system by using the geometric transformation method to calculate the relative spatial position vectors between the sensors. The time window pairing method is used to identify the corresponding peak points detected by adjacent sensors, and the propagation delay is calculated according to the timestamp information of the peak points. Combining the relative spatial position vectors of the sensors to obtain the torque propagation direction. For the propagation delay and position vector between each pair of adjacent sensors, the least squares method is used to fit the three-dimensional propagation trajectory curve, and the propagation direction is determined according to the tangent vector of the curve at each point. Combining the propagation delay to calculate the propagation speed magnitude. According to the fitted propagation trajectory curve, the unit tangent vector of the curve at each node is extracted as the propagation direction, and the propagation speed magnitude and the propagation direction are combined to obtain the propagation speed vector, forming a complete description of the torque peak propagation characteristics.

[0016] Further, it is determined whether the peak propagation speed vector exceeds a preset threshold. If it exceeds, the response trend of the joint torque controller is predicted by combining the magnitude and direction of the peak propagation speed and the peak amplitude change rate. When the predicted controller response leads to saturation, the stiffness and damping parameters are pre-adjusted in advance, including: setting the reference speed threshold for each of the x, y, and z directions according to the modulus and each direction component of the peak propagation speed vector, dynamically adjusting each direction threshold according to the joint motion state by using an adaptive method, and determining whether the speed vector exceeds the preset range to obtain an over-limit flag by comparing the magnitude relationship between each direction speed component and the corresponding threshold. The torque peak sequence is processed by using a sliding window gradient calculation method, the peak amplitude change rate is calculated according to the torque difference between adjacent sampling points, the change rate sequence is smoothed by low-pass filtering, and the response prediction parameter of the joint controller is calculated in combination with the over-limit flag. A feed-forward compensation matrix is established according to the response prediction parameter of the joint controller, the output response curve of the controller is predicted by using a polynomial fitting method, and the deviation between the predicted curve and the output limit value of the controller is calculated. For the deviation of the predicted curve, a sub-region judgment method is used to determine the controller saturation risk level, and the adjustment intervals of the stiffness parameter and the damping parameter are divided according to the saturation risk level. For the stiffness parameter interval, a linear interpolation method is used to calculate the stiffness adjustment amount, and dynamic compensation is performed on the basis of the original stiffness parameter to ensure that the compensated stiffness parameter is within the preset stable range. For the damping parameter interval, a proportional regulation method is used to calculate the damping adjustment amount, and the damping compensation coefficient is determined according to the adjustment result of the stiffness parameter to realize the coordinated adjustment of the stiffness parameter and the damping parameter.

[0017] Furthermore, according to the torque peak propagation speed vector and the amplitude change rate, optimize the stiffness and damping parameters of the impedance control model to obtain the optimized control parameters, including: constructing a radial basis function neural network according to the torque peak propagation speed vector and the amplitude change rate, setting the nodes of the network input layer as the three-dimensional components of the speed vector and the amplitude change rate, the number of hidden layer nodes being twice that of the input layer, and the nodes of the output layer being the adjustment amounts of the stiffness parameter and the damping parameter. Use the batch normalization method to preprocess the network input data, map the input data to a zero-mean unit-variance distribution through min-max normalization, and denormalize the parameter adjustment amount output by the network to restore it to the original numerical range. According to the normalized network input, use the gradient descent method to calculate the parameter adjustment direction, set the initial learning rate as the standard learning rate, and decrease the learning rate in each training cycle through the exponential decay method. Stop the iteration when the change value of the parameter adjustment amount is less than the convergence threshold. Perform a clipping process on the stiffness parameter adjustment amount after iterative optimization, set the upper limit of the stiffness parameter adjustment as plus or minus 30% of the original stiffness value, and use a piecewise linear function to map the adjustment amount outside the range to the limited interval. Perform a clipping process on the damping parameter adjustment amount after iterative optimization, set the change range of the damping parameter according to the adjustment result of the stiffness parameter, and keep the dynamic ratio of the stiffness parameter and the damping parameter within the preset range. Use the exponential moving average method to smooth the clipped parameter adjustment amount, set the smoothing coefficient as the standard value, and obtain the smoothed parameter adjustment sequence through recursive calculation. According to the parameter update period of the controller, perform timed sampling on the smoothed parameter adjustment sequence, and add the latest parameter adjustment amount to the original control parameters at each sampling moment to obtain the updated impedance control parameters.

[0018] Further, for the optimized control parameters, calculate the controller output response, and determine whether the amplitude of the controller output response is within the preset range. If it is within the preset range, output a stable control signal, including: constructing the impedance controller state equation according to the optimized stiffness parameter and damping parameter, numerically solving the state equation using the Runge-Kutta method, setting the solution step size as the sampling period, performing time-domain sampling on the obtained response sequence, and calculating the amplitude spectrum of the response signal through fast Fourier transform. Set the sliding window length to one-tenth of the sampling sequence length, perform window sliding calculation on the amplitude spectrum, record the mean and standard deviation within the window, and set the upper and lower amplitude threshold values according to the preset three-times standard deviation principle. Use the chi-square test method to perform a normality test on the amplitude distribution, set the significance level as the standard value, and judge whether the response signal conforms to the normal distribution characteristics according to the test results. For the response signal passing the normality test, calculate the correlation coefficient sequence of the signal using the autocorrelation function, fit the correlation coefficient decay curve through the exponential fitting method, and determine the signal stabilization time according to the characteristic time constant of the decay curve. Design a Butterworth low-pass filter with the cut-off frequency set to one-fourth of the sampling frequency, perform bandwidth limitation on the response signal, and use the zero-phase filtering method to eliminate phase distortion. Use the trapezoidal integration method to perform numerical integration on the filtered signal, set the integration step size as the sampling interval, and obtain a stable control output signal through the cumulative average method.

[0019] Further, obtain the elastic parameters of the mechanical structure, calculate the error influence of elastic deformation on torque measurement by using the finite element analysis method, and obtain the error compensation coefficient. Modify the stable control signal by the error compensation coefficient to obtain the final control output, including: establish a tetrahedral mesh structure model according to the structural parameters of the robotic arm and the installation position of the torque sensor, set the minimum size of the mesh element to one-tenth of the characteristic size, calibrate the elastic modulus and Poisson's ratio of the connecting rod material by the standard specimen loading method, and conduct a static stress analysis on the structural stress and deformation. Calculate the strain field distribution by using the Gaussian interpolation method, calculate the measurement error caused by the deformation according to the pre-calibrated sensitivity matrix of the torque sensor, and establish a mapping function between the load and the measurement error by cubic spline interpolation fitting. Calculate the error compensation coefficient according to the mapping function, perform continuous processing on the compensation coefficient by using the piecewise linear interpolation method, compensate and correct the stable control signal to obtain the corrected control quantity. Construct a feedforward compensation channel based on the corrected control quantity, calculate the feedforward gain matrix by the recursive least squares method, and optimize the parameters of the feedforward controller by the gradient descent method. Set the sliding window length to 100 times the sampling period, monitor the control output in real time, calculate the mean square deviation and the maximum deviation of the output signal as the stability index of the controller. Set the dynamic threshold according to the stability index, adaptively adjust the threshold by using the fuzzy rule, and judge whether the stability of the controller meets the preset requirements. Online store the control parameters that meet the stability requirements, update the parameter database by the exponential smoothing method, and form a complete parameter optimization record.

[0020] The present invention provides a humanoid robot motion coordination system based on joint torque, mainly including:

[0021] A torque data processing module, configured to obtain the original joint torque data of the humanoid robot through a sensor, preprocess the original torque data, and output stable torque data;

[0022] An impedance control modeling module, configured to obtain the robotic arm joint angle data of the humanoid robot through a sensor for the stable torque data, construct an impedance control model in combination with the robotic arm joint angle data, predict the torque change trend, and obtain a predicted torque sequence;

[0023] A peak detection module, configured to perform adaptive threshold detection on the predicted torque sequence to identify torque peaks of different amplitudes;

[0024] A propagation speed analysis module, configured to analyze the propagation speed of the torque peak by using a non-uniform sliding window method, and calculate the velocity vector of the peak propagation in combination with the direction of the torque peak propagation;

[0025] A response trend prediction module, which is used to determine whether the velocity vector of peak propagation exceeds a preset threshold. If it exceeds, it combines the magnitude and direction of the peak propagation velocity and the peak amplitude change rate to predict the response trend of the joint torque controller to determine whether it is necessary to adjust the humanoid robot joint control parameters

[0026] A parameter optimization module, which is used to optimize the stiffness and damping parameters of the impedance control model according to the torque peak propagation velocity vector and the amplitude change rate if it is necessary to adjust the humanoid robot joint control parameters, and obtain the optimized control parameters;

[0027] An output stability judgment module, which is used to calculate the amplitude of the controller output response signal for the optimized control parameters. If the amplitude of the controller output response signal is within the preset range, it outputs a stable control signal;

[0028] An error compensation module, which is used to obtain the structural parameters of the robotic arm, calculate the error influence of elastic deformation on the original torque data at the humanoid robot joints by using the finite element analysis method, and obtain an error compensation coefficient; correct the stable control signal through the error compensation coefficient to obtain the final control signal.

[0029] The technical solution provided by the embodiment of the present invention may include the following beneficial effects:

[0030] The present invention discloses a humanoid robot motion coordination method based on joint torque, which is used to improve the compliant adaptation ability and interaction stability of the robot in the case of accidental contact. This method first denoises and analyzes the frequency of the joint torque data, constructs an impedance control model and predicts the torque change trend. Then it identifies the torque peak through adaptive threshold detection and analyzes the peak propagation velocity vector by using a non-uniform sliding window. According to the propagation velocity and amplitude change rate, it dynamically optimizes the control parameters to avoid controller saturation. Finally, considering the influence of the elastic deformation of the mechanical structure, it compensates the error of the control signal to achieve stable compliant interaction control. The present invention can effectively cope with accidental contact situations and improve the safety and interaction performance of humanoid robots. BRIEF DESCRIPTION OF THE DRAWINGS

[0031] Figure 1 It is a flowchart of a humanoid robot motion coordination method based on joint torque of the present invention.

[0032] Figure 2 It is a structural schematic diagram of a humanoid robot motion coordination system based on joint torque of the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0033] Next, the technical solutions in the embodiments of the present invention will be clearly and detailedly described with reference to the accompanying drawings in the embodiments of the present invention. The described embodiments are only a part of the embodiments of the present invention.

[0034] The humanoid robot acquires motion data through joint torque sensors and angular velocity sensors, which are divided into media data and non-media data. The media data includes torque and angular velocity data, and the non-media data includes joint poses, etc. The control methods include impedance control based on torque and motion planning based on pose. Among them, impedance control realizes compliant interaction by adjusting stiffness and damping.

[0035] Figure 1 The implementation process of the humanoid robot motion coordination method based on joint torque provided by the embodiment of the present application is shown:

[0036] S101 obtains the original joint torque data of the humanoid robot through sensors. After preprocessing the original torque data, stable torque data is output.

[0037] The humanoid robot has a built-in motion coordination control function, which can be integrated into the robot operating system or a dedicated control module. The specific implementation method is determined according to the actual scenario. For example, the control function can be started through the user operation interface, or remotely triggered by an external device sending an instruction. The present invention does not limit the start method too much and can be set by technicians according to requirements. For example, in an industrial scenario, the user can start the control function through the touch interface of the robot control panel; in a service scenario, it can be triggered by a voice command.

[0038] S1011 After the control function is started, the humanoid robot acquires joint point data through joint torque sensors and angular velocity sensors, obtains the original torque data and the original angular velocity data according to the preset sampling frequency, and determines the torque data to be processed.

[0039] The torque sensor acquires joint torque data in a high-frequency sampling manner, and the angular velocity sensor synchronously records the joint motion state. The sampling frequency can be set according to the task requirements. For example, in a high-dynamic task, the sampling frequency of the torque sensor can be set to 1000 Hz, and the angular velocity sensor is 200 Hz. The torque data to be processed can be obtained by segmenting through a time window. For example, every 2 seconds is used as a window to generate a data set containing multiple sampling points. When determining the data to be processed, it can be automatically selected according to the task type. For example, in a grasping task, the torque data of the elbow joint and the wrist joint are preferentially selected.

[0040] S1012 For the acquired original torque data, the humanoid robot identifies whether it is media data, and performs stability analysis on the angular velocity data to screen out the torque data corresponding to the stable time period.

[0041] Media data refers to dynamic data such as torque and angular velocity, and non-media data includes static poses, etc. The identification method can be achieved through data type marking or format analysis. For example, torque data is usually stored in the form of a time series and can be judged through the file header information. If it is confirmed as media data, the mean and standard deviation of each joint point are calculated for the angular velocity data, and the standard deviation is compared with a preset threshold, such as 0.05 radians per second. If the standard deviation is lower than the threshold, the corresponding time period is marked as a stable time period, and the torque data during this period is extracted as the data to be processed.

[0042] S1013 performs denoising processing on the filtered torque data using a multi-scale decomposition method, extracts noise characteristics by combining frequency domain analysis, judges whether the noise residue meets the preset conditions, and outputs stable torque data.

[0043] The denoising processing uses wavelet transform technology. A wavelet basis function suitable for high-frequency noise filtering, such as db4, is selected for three-layer decomposition. The first layer of decomposition extracts high-frequency noise of 50 - 500 Hz, the second layer corresponds to medium-frequency vibration of 10 - 50 Hz, and the third layer contains low-frequency changes below 10 Hz. The decomposition coefficients of each layer are screened according to the energy threshold. For example, coefficients with an energy ratio lower than 5% are removed, and then signal reconstruction is performed to obtain denoised torque data. To ensure the denoising effect, a fast Fourier transform is used to generate a spectrogram, the frequency components with an amplitude greater than the preset threshold are extracted, and their energy ratio is calculated as the noise residue feature. If the eigenvalue is lower than 3%, the denoising effect is considered good. The denoised data is smoothed, the sliding window length is set to one-thousandth of the number of sampling points, and the mean value is calculated to suppress short-term fluctuations. If the torque change rate of a certain sampling point exceeds 5%, it is marked as an abnormal point and corrected using the median of the previous and next three sampling points.

[0044] Through multi-scale denoising and frequency domain analysis, high-frequency noise and outliers in the torque data can be effectively filtered, ensuring the continuity and reliability of the output data. For example, when the robot performs a 3-kg load handling task, the steady-state torque of the elbow joint is about 15 N·m, and the fluctuation is controlled within 0.3 N·m, meeting the high-precision control requirements. Compared with traditional filtering methods, the embodiment of the present invention improves the data quality, provides a reliable basis for impedance control, and thus optimizes the motion coordination performance of the robot in dynamic interaction.

[0045] S102 obtains the mechanical arm joint angle data of the humanoid robot through sensors. The mechanical arm joint angle data combines with the stable torque data to construct an impedance control model and predicts the torque change trend to obtain a predicted torque sequence.

[0046] When it is confirmed that the torque data to be processed is media data, the humanoid robot analyzes the joint torque and angular velocity data to construct a dynamic impedance control model to achieve compliant interaction. Impedance control adjusts the stiffness and damping parameters to establish the mapping relationship between joint torque and position, ensuring that the robot can flexibly respond to external disturbances. In this step, the Kalman filter algorithm is used to predict the torque change trend, generate a high-precision torque prediction sequence, and at the same time verify and optimize the model parameters to reduce the control error and improve the motion coordination. The present invention does not impose too many limitations on the specific model structure, which can be set by technicians according to task requirements.

[0047] S1021 Based on the stable torque data and joint angle data, the humanoid robot constructs an initial impedance control parameter matrix, and defines the mapping relationship between torque and position through the stiffness and damping coefficients.

[0048] The impedance control parameter matrix is generated by the stable torque data and the joint angle data collected in real time. The stiffness coefficient reflects the torque response of the joint to the position deviation, and the damping coefficient controls the damping force of the speed change. For example, in an industrial assembly task, the stiffness coefficient of the elbow joint can be set to 200 to 400 Nm / rad, and the damping coefficient can be set to 20 to 40 Nm / (rad·s). The mapping relationship is represented by a linear equation, where the torque is equal to the stiffness coefficient multiplied by the position deviation plus the damping coefficient multiplied by the speed deviation. The initial matrix assigns independent stiffness and damping parameters to each joint, forming a multi-dimensional parameter set. The data acquisition frequency can be set to 1000 Hz to ensure real-time performance.

[0049] S1022 For the initial impedance control parameter matrix, a dynamic response test method is used to verify the impedance characteristics, and the parameters are corrected according to the error index to obtain an optimized parameter matrix.

[0050] To verify the impedance characteristics, a unit step response test is adopted. A preset torque input, such as 50 Nm, is applied to the joint, and the dynamic response of the joint angle is observed. Calculate the steady-state error and overshoot. For example, it is required that the steady-state error is lower than 0.02 rad and the overshoot is less than 15%. If the error exceeds the threshold, adjust the stiffness and damping coefficients. For example, increasing the stiffness coefficient of the elbow joint to 300 Nm / rad and adjusting the damping coefficient to 30 Nm / (rad·s) can reduce the overshoot to 10% and the steady-state error to 0.015 rad. The correction process is achieved through iterative optimization to generate an optimized parameter matrix. The present invention does not impose too many limitations on the test method, and other dynamic response analysis methods can be selected according to the hardware performance.

[0051] S1023 Based on the optimized parameter matrix and real-time position feedback, the humanoid robot establishes a motion state space model, selects the joint angle, angular velocity, and torque as state variables, and constructs an observation relationship.

[0052] The state - space model uses joint angle, angular velocity, and torque as state variables and describes the system dynamics through state equations. The observation matrix defines the relationship between state variables and sensor measurement values, such as the outputs of torque sensors and angular velocity sensors. The diagonal elements of the initialized process noise covariance matrix are set to 0.0001, and the measurement noise covariance matrix is 0.001 to ensure the robustness of the model to noise. The state equation uses a sampling period of 0.001 seconds to ensure high - frequency response. After the model is constructed, it can be used for subsequent state estimation and prediction. The present invention does not impose excessive restrictions on the selection of state variables, and the variables can be increased or decreased according to the task complexity.

[0053] S1024 For the state - space model, the Kalman filter algorithm is used to calculate the state estimation value, generate future state prediction values based on the prediction step, and extract the torque component to form a torque prediction sequence.

[0054] The Kalman filter algorithm realizes state estimation through iterative updates. First, the state at the next moment is predicted according to the state equation, and the residual between the predicted value and the actual measurement value is calculated in combination with the observation matrix. The Kalman gain is calculated based on the residual and the noise covariance and is used to correct the state estimation value. For example, the joint angle estimation error is controlled within 0.005 radians, and the torque error is less than 0.5 N·m. The prediction step is set to 100 sampling points, that is, 0.1 seconds, to generate future state prediction values. The torque component in the prediction value is extracted to form a torque prediction sequence. The sequence is processed by a sliding window with a window length of 200 sampling points, and the mean - square error with the actual torque sequence is calculated. If the error exceeds 1 N·m, the noise covariance matrix of the state - space model is adjusted. For example, the process noise covariance is increased to 0.0002, and the measurement noise covariance matrix is reduced to 0.0005 to improve the prediction accuracy.

[0055] S1025 Conduct error analysis on the torque prediction sequence. If the error exceeds the preset threshold, the parameters of the state - space model are dynamically updated, and state estimation and torque prediction are performed again.

[0056] Error analysis is realized by comparing the mean - square error between the prediction sequence and the actual torque sequence. For example, in a precision assembly task, the torque variation range is 20 to 30 N·m, and the target mean - square error is less than 0.8 N·m. If the error exceeds the standard, the noise covariance matrix of the state - space model is updated, and the Kalman filter algorithm is executed again. The adjusted model can reduce the error to 0.6 N·m and improve the prediction accuracy. The update process can be realized by an adaptive algorithm, such as optimizing the covariance parameters based on gradient descent. The present invention does not impose excessive restrictions on the error analysis method, and other statistical methods can be selected according to the task requirements.

[0057] By constructing a dynamic impedance control model and Kalman filter prediction, the trend of torque change can be accurately captured, providing reliable data for peak analysis and control optimization. For example, in the pin insertion task, when the robotic arm inserts at a speed of 0.05 m / s, the error of the predicted torque sequence of the elbow joint remains within 0.6 N·m, ensuring stable contact force. Compared with traditional static impedance control, the embodiments of the present invention significantly improve the control accuracy and response speed in dynamic scenarios, providing a technical guarantee for the compliant interaction of humanoid robots.

[0058] S103 performs adaptive threshold detection on the predicted torque sequence to identify torque peaks of different amplitudes.

[0059] For the predicted torque sequence, the humanoid robot adopts an adaptive threshold detection method to identify the torque peaks caused by unexpected contact, and extracts the peak amplitude and timestamp information. By analyzing the torque change rate through a sliding window, combining the dynamic threshold boundary and clustering method, a dataset of torque peaks with classification labels is generated. This method can accurately capture peaks of different amplitudes, adapt to complex interaction scenarios, and improve the robot's response ability to external disturbances. The present invention does not limit the specific implementation of the detection algorithm too much and can be adjusted according to the hardware performance and task requirements.

[0060] S1031 Based on the predicted torque sequence, the humanoid robot calculates the torque change rate and performs normalization processing to generate a feature sequence to support peak detection.

[0061] The torque change rate is obtained by calculating the torque difference between adjacent sampling points, reflecting the dynamic change trend of the torque. A sliding window with a fixed length of 50 sampling points, equivalent to 50 milliseconds, is used to calculate the mean value of the torque change rate within the window, ensuring that rapid changes are captured while filtering out instantaneous noise. The change rate sequence is mapped to the interval from 0 to 1 through the maximum-minimum normalization method to form a normalized torque change feature sequence. The normalization processing unifies the data scales of different joints, facilitating subsequent threshold detection. For example, in the assembly task, after the normalization of the torque change rate of the elbow joint, the normal fluctuation range is between 0.1 and 0.3, and the abnormal peak may be close to 0.8.

[0062] S1032 For the normalized torque change feature sequence, the humanoid robot generates a dynamic threshold boundary, marks the data points beyond the boundary, and divides the candidate peak intervals.

[0063] The dynamic threshold is generated by calculating the local mean and standard deviation of a 100 - sample - point window with a window length of 100 milliseconds, which balances detection sensitivity and stability. The threshold boundary is set to the local mean plus or minus three times the standard deviation, which can adapt to the slow change of the torque baseline while detecting significant anomalies. For example, in the pin - fitting task, the torque fluctuations caused by slight collisions usually exceed the upper threshold boundary. The data points exceeding the boundary are marked and segmented into time windows of 500 milliseconds to form candidate torque peak intervals. The segmentation length is based on the typical collision duration. For example, the collision between the robotic arm and the workpiece usually lasts for 200 to 400 milliseconds, ensuring that the interval completely covers the peak event.

[0064] S1033 For the candidate torque peak intervals, the humanoid robot extracts valid peak points, judges the validity of the wave peaks based on the amplitude difference, and generates a preliminary peak data set.

[0065] Each candidate interval identifies the maximum and minimum points through a peak - detection algorithm and calculates the amplitude difference between adjacent extreme points. If the difference is less than 2 N·m, it is regarded as normal fluctuation rather than a valid peak; if the difference is greater than 5 N·m, it is marked as a candidate peak point and the timestamp is recorded with a time resolution of 1 millisecond. For example, workpiece jamming may generate a peak of 10 N·m, which is significantly different from the slight rubbing fluctuation of 3 N·m. The extracted peak points contain amplitude and time information, forming a preliminary torque peak data set, which provides a basis for classification.

[0066] S1034 uses a clustering method to classify the valid peak points and generates an annotated torque peak data set containing amplitude category labels and timestamps.

[0067] The k - means clustering algorithm based on Euclidean distance is used to classify the peak points. Three amplitude intervals are preset: slight rubbing is below 5 N·m, workpiece jamming is between 5 and 15 N·m, and severe collision is above 15 N·m. The clustering centers are usually near 3 N·m, 10 N·m, and 20 N·m, reflecting typical contact scenarios. Each peak point is assigned a category label, and the torque value and timestamp are recorded to generate an annotated data set. For example, within an assembly cycle, 10 slight - rubbing peaks and 4 jamming peaks may be recorded, and severe collisions rarely occur. The data set supports subsequent analysis of collision patterns and optimization of the robot path planning.

[0068] The torque peak detection and classification method can accurately identify accidental contact events in a dynamic environment. For example, when a six - axis robotic arm executes an assembly task at a speed of 0.2 m / s, the normal torque fluctuation of the elbow joint is between 15 and 25 N·m, and the detection method can identify 95% of the normal data and abnormal peaks. Compared with the traditional fixed - threshold method, the embodiments of the present invention significantly improve the accuracy and robustness of peak detection through dynamic threshold and clustering analysis, providing reliable data support for the humanoid robot to achieve safe interaction.

[0069] S104 uses a non-uniform sliding window method to analyze the propagation speed of torque peaks, and calculates the velocity vector of peak propagation in combination with the direction of torque peak propagation.

[0070] For the torque peak dataset with classification labels, the humanoid robot uses a non-uniform sliding window method to analyze the propagation speed and direction of torque peaks in the robotic arm structure. The method dynamically adjusts the window size according to the severity of torque changes, and through weighted processing and vector analysis, generates a propagation feature description including the magnitude and direction of the speed. This method can accurately capture the dynamic characteristics of peak propagation, providing a basis for subsequent control optimization and collision localization. The present invention does not overly limit the window adjustment strategy or analysis method, which can be adjusted according to actual scenario requirements.

[0071] S1041 Based on the torque peak data, the humanoid robot calculates the torque change rate sequence and adopts an adaptive window adjustment strategy to optimize the analysis granularity.

[0072] The torque change rate is calculated by the torque difference between adjacent sampling points, reflecting the instantaneous dynamics of peak propagation. The reference sliding window size is set to 100 milliseconds, which is equivalent to 100 sampling points and is suitable for capturing conventional torque changes. Using a gradient-based adaptive method, if the change rate exceeds 20% of the peak amplitude, the window is reduced to 25 milliseconds to improve the sensitivity to transient peaks; if the change rate is below 5%, the reference window is maintained to suppress high-frequency noise interference. For example, when the robotic arm grasps a workpiece, a slight collision may cause the change rate to rise to 25%, triggering the window to shrink, thus accurately capturing the initial stage of peak propagation. This dynamic adjustment balances the detection accuracy and computational efficiency.

[0073] S1042 Normalize the torque peak points and assign weights to generate a weighted torque peak sequence to highlight important events.

[0074] The torque peak amplitude is normalized by the maximum peak and mapped to the interval from 0 to 1, and then converted into a weight coefficient from 0.5 to 1.5 through a linear function. For example, a peak of 20 N·m is normalized to 0.8 and mapped to a weight of 1.3; a peak of 5 N·m is normalized to 0.2 and mapped to a weight of 0.7. The weight coefficient is multiplied by the normalized peak to form a weighted torque peak sequence. The weighted processing highlights the contribution of high-amplitude peaks to the analysis of propagation characteristics. For example, a severe collision event has a higher influence compared to a slight rubbing. This method improves the sensitivity of the analysis to key events and provides high-quality input for vector calculation.

[0075] S1043 Establish a three-dimensional coordinate system and map the sensor positions. The humanoid robot calculates the peak propagation time delay and spatial trajectory through time window pairing.

[0076] A three-dimensional rectangular coordinate system is established with the base of the robotic arm as the origin, and the sensor positions are mapped to a unified coordinate system through geometric transformation. For example, the elbow joint sensor is located at coordinates 400 mm, 0 mm, 300 mm, and the wrist joint sensor is located at 800 mm, 0 mm, 400 mm. A 50-millisecond time window is used to pair the peak points detected by adjacent sensors, and the time stamp difference is calculated as the propagation delay. For example, the delay from the end effector to the wrist joint is approximately 15 milliseconds, and from the wrist joint to the elbow joint is approximately 20 milliseconds. Combining the relative position vectors between sensors, a spatial trajectory description of the propagation is generated.

[0077] S1044 uses the least squares method to fit the propagation trajectory, and the humanoid robot calculates the propagation speed vector of the torque peak to generate a complete propagation feature.

[0078] The least squares method is used to fit the time and position data of the peak points to generate a three-dimensional propagation trajectory curve. The tangent vector of the curve represents the propagation direction, and the speed magnitude is calculated by combining the time delay. For example, the propagation speed of the torque peak in the robotic arm is approximately 30 m / s, of which the horizontal component is approximately 25 m / s and the vertical component is approximately 15 m / s. The combination of the speed magnitude and direction forms a propagation speed vector, which completely describes the peak propagation characteristics. Different collision types, such as end effector collision or joint jamming, produce unique vector features, supporting event classification and localization. For example, in a grasping task at 0.5 m / s, the propagation trajectory of a slight collision shows a clear directionality, helping to identify the source of the collision.

[0079] The non-uniform sliding window and vector analysis method significantly improves the accuracy of the torque peak propagation analysis. For example, when a six-axis robotic arm performs an assembly task, the calculation error of the torque peak propagation speed is controlled within 5%, and the direction estimation deviation is less than 10 degrees. Compared with the fixed window method, the embodiments of the present invention better adapt to complex dynamic scenarios through dynamic adjustment and weighting processing, providing key support for optimizing the control strategy of humanoid robots and improving interaction safety.

[0080] S105 determines whether the speed vector of the peak propagation exceeds a preset threshold. If it exceeds, then combining the magnitude and direction of the peak propagation speed, and the peak amplitude change rate, predicts the response trend of the joint torque controller to determine whether it is necessary to adjust the joint control parameters of the humanoid robot.

[0081] For the torque peak propagation speed vector, the humanoid robot evaluates whether it exceeds a preset threshold, and predicts the controller response trend by combining the peak amplitude change rate. If the prediction shows that the controller may saturate, the stiffness and damping parameters are optimized through feedforward compensation and parameter adjustment to ensure motion coordination and safety. This method can respond to abnormal torques in dynamic interactions in real time, improving the robustness of the robot's compliant control. The present invention does not overly limit the threshold setting or adjustment algorithm, and can be flexibly configured according to task requirements.

[0082] Based on the peak propagation speed vector, the humanoid robot compares the component in each direction with the dynamic threshold to generate an over-threshold flag to identify abnormal propagation.

[0083] The x, y, and z components of the propagation speed vector are respectively compared with the reference thresholds in the corresponding directions. For example, the x direction is set to 20 meters per second, and the y and z directions are set to 15 meters per second. The threshold is dynamically adjusted according to the joint motion state through an adaptive mechanism. For example, when the speed component in the x direction reaches 25 meters per second, the threshold is temporarily increased to 30 meters per second. If any component exceeds the threshold, an over-threshold flag is generated to mark a potential abnormal propagation event. The comparison process has a period of 1 millisecond to ensure high real-time performance. This dynamic threshold design can adapt to the torque propagation characteristics under different working conditions, such as the sudden increase in radial speed caused by workpiece tilt during precision assembly.

[0084] S1052 performs gradient analysis on the torque peak sequence. The humanoid robot calculates the controller response prediction parameters in combination with the over-threshold flag.

[0085] A 20-millisecond sliding window is used to calculate the rate of change of the amplitude of the torque peak sequence, and a rate-of-change sequence is generated through the torque difference between adjacent sampling points. To reduce the influence of noise, the rate-of-change sequence is smoothed by a low-pass filter with a cut-off frequency of 50 Hertz. For example, in a minor collision, the rate of change may reach 200 Newton-meters per second, and after filtering, it can more accurately reflect the dynamic trend. In combination with the over-threshold flag, the response prediction parameters are calculated to reflect the response sensitivity of the controller to torque changes. The parameters include the peak value and duration of the rate of change, providing inputs for the saturation risk assessment.

[0086] S1053 constructs a feed-forward compensation matrix based on the response prediction parameters. The humanoid robot evaluates the controller saturation risk through polynomial fitting.

[0087] The feed-forward compensation matrix takes joint position, velocity, and torque as inputs and predicts the controller output response curve through 6th-order polynomial fitting. The deviation between the predicted curve and the controller output limit value is compared. If the deviation exceeds 20%, it is considered that there is a saturation risk. The deviation is divided into intervals: 10% to 20% is a low risk, 20% to 40% is a medium risk, and more than 40% is a high risk. For example, a deviation of 30% indicates a medium risk, suggesting that the control parameters need to be adjusted. The fitting process considers multi-dimensional state variables to ensure the prediction accuracy. The matrix construction supports real-time updates to adapt to dynamic interaction scenarios.

[0088] S1054 adjusts the stiffness and damping parameters according to the saturation risk level. The humanoid robot achieves coordinated compensation through linear interpolation and proportional regulation.

[0089] The stiffness parameter adjustment is based on the risk level, and the intervals are ±10% for low risk, ±20% for medium risk, and ±30% for high risk. The adjustment amount is calculated by linear interpolation. For example, the original stiffness of 400 N·m / rad can be adjusted to 320 - 480 N·m / rad under medium risk. The damping parameter is adjusted proportionally and maintains the same ratio as the stiffness. For example, when the stiffness is reduced by 20%, the damping is reduced from 40 N·m / (rad·s) to 32 N·m / (rad·s). The coordinated adjustment ensures the stability of the impedance characteristics. For example, in the bearing assembly, an eccentric contact induces a radial velocity of 28 m / s. After adjustment, the stiffness is reduced to 350 N·m / rad and the damping is reduced to 35 N·m / (rad·s), effectively avoiding saturation.

[0090] The dynamic parameter adjustment method significantly improves the stability of the controller in complex interactions. For example, when a six-axis robotic arm performs a part insertion task at a speed of 0.05 m / s, the abnormal propagation speed of the torque peak triggers parameter adjustment, the control error is kept within 0.5 N·m, and the response time is shortened to 50 ms. Compared with static parameter control, the embodiments of the present invention better handle unexpected contact scenarios through feedforward compensation and adaptive adjustment, providing technical support for high-precision compliant interaction of humanoid robots.

[0091] S106 If the joint control parameters of the humanoid robot need to be adjusted, the stiffness and damping parameters of the impedance control model are optimized according to the torque peak propagation speed vector and the amplitude change rate to obtain the optimized control parameters.

[0092] Based on the torque peak propagation speed vector and the amplitude change rate, the humanoid robot dynamically optimizes the impedance control parameters using a radial basis function neural network, and adjusts the stiffness and damping in real time through an adaptive algorithm to ensure the compliance and stability of the robot in unexpected contact scenarios. The optimization process includes data preprocessing, parameter iteration, clipping, and smoothing processing, and finally generates high-precision control parameters. The present invention does not overly limit the neural network structure or the optimization algorithm, and can be flexibly adjusted according to the computing resources and task complexity.

[0093] S1061 Based on the torque peak propagation speed vector and the amplitude change rate, the humanoid robot constructs the neural network input data and performs normalization processing to improve the training stability.

[0094] The neural network input data includes the x, y, z components of the velocity vector, the resultant velocity, as well as the amplitude change rate and its three-direction components and the resultant change rate, a total of 8-dimensional data. The batch normalization method is used to standardize the input data into a distribution with a mean of 0 and a variance of 1. For example, the velocity component of 25 m / s and the amplitude change rate of 200 N·m / s are mapped to the standard interval through min-max normalization. The normalization processing unifies the data scale, reduces the risk of gradient explosion during the training process, and ensures that the network can converge quickly. The preprocessed data provides high-quality input for subsequent optimization.

[0095] S1062 uses a radial basis function neural network and the gradient descent method. The humanoid robot iteratively calculates the parameter adjustment direction and adjusts the learning rate through exponential decay.

[0096] The radial basis function neural network is configured with 8 input nodes, 16 hidden layer nodes, and 2 output nodes, corresponding to the adjustment amounts of stiffness and damping parameters respectively. The gradient descent method calculates the gradient of the loss function based on the normalized input to determine the parameter adjustment direction. The initial learning rate is set to 0.01 and decays to 0.8 times every 100 training cycles to balance the convergence speed and accuracy. When the change in the parameter adjustment amount for 5 consecutive cycles is less than 0.001, the iteration stops. Exponential decay ensures rapid optimization in the early stage and refined adjustment in the later stage. For example, in bearing assembly, the network can quickly respond to the x-direction speed peak of 28 m / s and generate reasonable parameter adjustment amounts.

[0097] S1063 performs amplitude limiting and smoothing processing on the iterative parameters. The humanoid robot generates a stable parameter adjustment sequence and updates it to the impedance controller.

[0098] The adjustment amount of the stiffness parameter is amplitude-limited within plus or minus 30% of the original value. For example, if the original stiffness is 400 N·m / rad, the adjustment range is 280 to 520 N·m / rad. If the calculated adjustment amount exceeds the range, it is mapped to the limited interval through a piecewise linear function. The damping parameter is amplitude-limited synchronously according to the stiffness adjustment ratio. For example, when the stiffness drops to 350 N·m / rad, the damping range is 28 to 42 N·m / (rad·s), maintaining the stiffness-damping ratio between 8 and 12. After amplitude limiting, the exponential moving average method is used for smoothing processing. The smoothing coefficient is 0.8, with the current value accounting for 20% and the historical value accounting for 80%, generating a smoothed parameter sequence. With a 5-ms update period, the sampling sequence is superimposed on the original parameters to complete the controller update.

[0099] The neural network optimization method significantly improves the real-time performance and accuracy of parameter adjustment. For example, when a six-axis robotic arm performs a bearing assembly task and detects an x-direction propagation speed of 28 m / s and an amplitude change rate of 180 N·m / s, the network outputs a stiffness adjustment amount of -80 N·m / rad, which is actually adjusted to -60 N·m / rad after amplitude limiting and smoothing. The damping drops from 40 N·m / (rad·s) to 34 N·m / (rad·s), and the adjustment process is completed within 15 ms. Compared with traditional fixed-parameter adjustment, the embodiments of the present invention effectively cope with complex scenarios such as workpiece interference through multi-dimensional feature input and dynamic optimization, ensuring stable interaction performance and control accuracy.

[0100] S107 obtains the structural parameters of the robotic arm, uses the finite element analysis method to calculate the error influence of elastic deformation on the original data of the torque at the joints of the humanoid robot, and obtains the error compensation coefficient; corrects the stable control signal through the error compensation coefficient to obtain the final control signal.

[0101] Based on the optimized stiffness and damping parameters, the humanoid robot numerically solves the state equation of the impedance controller to generate a response sequence, and uses frequency-domain analysis and filtering techniques to verify the stability of the output signal. If the response amplitude is within the preset range, a stable control signal is output to ensure the coordination and safety of the robot's movements. The present invention does not impose excessive limitations on the solution method or filter design, and can be flexibly adjusted according to task requirements and hardware performance.

[0102] S1071 Calculate the amplitude of the controller output response signal for the optimized control parameters. If the amplitude of the controller output response signal is within the preset range, output a stable control signal.

[0103] The state equation of the impedance controller takes the optimized stiffness and damping parameters as inputs, such as a stiffness of 350 N·m / rad and a damping of 35 N·m / (rad·s). The fourth-order Runge-Kutta method is used for numerical solution, with a step size set to 1 ms, to generate a high-precision time-domain response sequence that reflects the dynamic response of the controller to torque input. Apply the fast Fourier transform to the sequence to generate an amplitude spectrum from 0 to 500 Hz and analyze the frequency distribution characteristics. For example, in a bearing press-fitting task, under normal operating conditions, the main frequencies are concentrated below 50 Hz, indicating that the response has a low-pass characteristic. The frequency-domain analysis provides basic data for stability verification.

[0104] S1072 Analyze the amplitude spectrum through a sliding window. The humanoid robot uses the chi-square test to verify the normality of the response signal and calculate the correlation coefficient sequence.

[0105] Set a 200-ms sliding window, which is approximately one-tenth of the sampling sequence, and calculate the mean and standard deviation of the amplitude spectrum. For example, the mean is approximately 25 N·m and the standard deviation is less than 2 N·m. Set the threshold to the mean plus or minus three times the standard deviation, i.e., 19 to 31 N·m. Use the chi-square test with a significance level of 0.05 to verify whether the amplitude distribution conforms to a normal distribution. Under normal conditions, the skewness coefficient is less than 0.3 and the kurtosis coefficient is between 2.8 and 3.2, indicating that the signal is stable. Further calculate the correlation coefficient sequence through the autocorrelation function, fit an exponentially decaying curve, extract the characteristic time constant of approximately 80 ms, and determine the stable time to be 240 ms. This multi-dimensional analysis ensures a comprehensive assessment of signal stability.

[0106] S1073 Design a low-pass filter to limit the bandwidth of the response signal. The humanoid robot generates a stable control output signal through numerical integration.

[0107] An 8th-order Butterworth low-pass filter is adopted, with a cut-off frequency of 125 Hz, which is about one-fourth of the sampling frequency, effectively suppressing high-frequency noise above 200 Hz, such as high-frequency oscillations caused by bearing jamming. The zero-phase filtering technology eliminates phase distortion and maintains the time characteristics of the signal. The trapezoidal integration method is used for the filtered signal, with a step size of 1 ms, and the output is controlled by cumulative averaging. For example, during a 2-second press-fitting process, the fluctuation of the output signal is controlled within plus or minus 5% of the nominal value to ensure stability. The filtering and integration processing improve the robustness of the signal and adapt to complex working conditions.

[0108] The stability analysis and signal processing method significantly improve the reliability of the control output. For example, when a six-axis robotic arm performs a bearing press-fitting task, the positioning accuracy error is controlled within 0.02 mm, and the accuracy rate of stability judgment exceeds 98%. Compared with the traditional single-threshold method, the present invention effectively copes with anomalies such as assembly surface collisions through multi-stage analysis and filtering processing, ensuring high qualification rate and motion coordination of the humanoid robot during continuous operation.

[0109] S108 Obtain the structural parameters of the robotic arm, calculate the error influence of elastic deformation on the original torque data at the joints of the humanoid robot by using the finite element analysis method, and obtain the error compensation coefficient; correct the stable control signal through the error compensation coefficient to obtain the final control signal.

[0110] Based on the structural characteristics of the robotic arm, the humanoid robot uses the finite element analysis method to quantify the torque measurement error caused by elastic deformation, generates a compensation coefficient to correct the control signal. Through the feedforward control algorithm and real-time stability monitoring, the impedance control model is optimized, and the final control signal is output to drive the joints to perform stable interaction. The present invention does not make too many limitations on the specific implementation of the finite element model or the compensation algorithm, and can be flexibly adjusted according to the robotic arm material and task requirements.

[0111] S1081 Based on the structural parameters of the robotic arm, the humanoid robot calculates the elastic deformation of the connecting rod by using the finite element analysis and generates stress-strain distribution data.

[0112] The connecting rod of the robotic arm is made of aluminum alloy material, with an elastic modulus of about 70 GPa and a Poisson's ratio of 0.33. A tetrahedral mesh model is established, and the minimum size of the mesh element is one-tenth of the characteristic size of the connecting rod, such as 5 mm, to ensure the calculation accuracy. The material parameters are calibrated by the standard specimen loading method, a 50 N·m load is applied for static stress analysis, and stress distribution data is generated. The stress analysis shows that about 0.1 mm deformation occurs near the sensor installation position, resulting in torque measurement deviation. The stress-strain data provides high-precision input for error calculation and is applicable to structural analysis under various working conditions.

[0113] S1082 According to the strain field distribution and the sensor sensitivity, the humanoid robot calculates the measurement error and establishes the mapping relationship between the load and the error.

[0114] The stress distribution data is processed using the Gaussian interpolation method to generate a continuous strain field distribution, accurately describing the deformation characteristics of the sensor position. Combining with the pre-calibrated sensitivity matrix of the torque sensor, the measurement error caused by deformation is calculated. For example, the error reaches 3% to 5% under a load of 50 N·m. The mapping function between the load and the error is fitted by cubic spline interpolation, and the fitting accuracy is better than 0.1% in the range of 0 to 100 N·m. The mapping function reveals the non-linear characteristics of the error. The compensation coefficient in the low-load area is about 1.03, and it rises to 1.05 in the high-load area. This mapping relationship provides a theoretical basis for error compensation.

[0115] S1083 Based on the error mapping function, the humanoid robot generates continuous compensation coefficients and corrects the stable control signal through feedforward control.

[0116] The mapping function is made continuous by piecewise linear interpolation to generate a smooth error compensation coefficient curve, avoiding sudden changes during the compensation process. The compensation coefficient is applied to the stable control signal. For example, the measured torque is multiplied by a coefficient of 1.03 to 1.05 to generate a corrected control quantity. A feedforward compensation channel is constructed, and the feedforward gain matrix is calculated by the recursive least squares method. The forgetting factor is set to 0.98 to ensure a fast response to new data. The gain parameters are optimized by the gradient descent method with a learning rate of 0.01, and the parameters are stable after about 1000 iterations. The corrected signal effectively reduces the influence of elastic deformation.

[0117] S1084 By real-time monitoring of the control output, the humanoid robot evaluates the stability and updates the parameter database to support continuous optimization.

[0118] A 500-ms sliding window is set to monitor the control output, and the mean square error and the maximum deviation are calculated as stability indicators. Under normal working conditions, the mean square error is controlled within 2% of the nominal value, and the maximum deviation is less than 5%. Fuzzy rules are used to adaptively adjust the threshold. For example, the mean square error threshold is tightened to 1% during precision assembly and relaxed to 3% during high-speed movement. The parameters that meet the stability requirements are stored by the exponential smoothing method with a smoothing coefficient of 0.95, highlighting the recent optimization results while retaining the historical trend. The parameter database supports online update, providing an optimization record for long-term operation.

[0119] Elastic error compensation and real-time monitoring significantly improve the torque control accuracy. For example, when a six-axis robotic arm performs a pin insertion task, the position error is controlled within 0.02 mm, and the torque accuracy is better than 0.5 N·m. Compared with the uncompensated control method, the present invention effectively copes with disturbances such as workpiece deviations through finite element analysis and feedforward compensation, and maintains the parameter fluctuation within plus or minus 2% during an 8-hour continuous assembly task, demonstrating excellent adaptability and stability.

[0120] As Figure 2, the present invention provides a humanoid robot motion coordination system based on joint torque, mainly including:

[0121] A torque data processing module, which is used to obtain the original joint torque data of the humanoid robot through sensors, preprocess the original torque data, and output stable torque data;

[0122] An impedance control modeling module, which is used to obtain the joint angle data of the robotic arm of the humanoid robot through sensors, construct an impedance control model by combining the mechanical arm joint angle data with the stable torque data, and predict the torque change trend to obtain a predicted torque sequence;

[0123] A peak detection module, which is used to perform adaptive threshold detection on the predicted torque sequence to identify torque peaks of different amplitudes;

[0124] A propagation speed analysis module, which is used to analyze the propagation speed of the torque peak by using a non-uniform sliding window method, and calculate the speed vector of the peak propagation by combining the direction of the torque peak propagation;

[0125] A response trend prediction module, which is used to judge whether the speed vector of the peak propagation exceeds a preset threshold. If it exceeds, then combine the magnitude and direction of the peak propagation speed, and the peak amplitude change rate, to predict the response trend of the joint torque controller to judge whether it is necessary to adjust the joint control parameters of the humanoid robot;

[0126] A parameter optimization module, which is used to, if it is necessary to adjust the joint control parameters of the humanoid robot, optimize the stiffness and damping parameters of the impedance control model according to the torque peak propagation speed vector and the amplitude change rate to obtain optimized control parameters;

[0127] An output stability judgment module, which is used to calculate the amplitude of the controller output response signal for the optimized control parameters. If the amplitude of the controller output response signal is within a preset range, then output a stable control signal;

[0128] An error compensation module, which is used to obtain the structural parameters of the robotic arm, calculate the error influence of the elastic deformation on the original torque data at the joints of the humanoid robot by using the finite element analysis method to obtain an error compensation coefficient; correct the stable control signal by the error compensation coefficient to obtain the final control signal.

[0129] The above description is only a preferred embodiment of the present application and an explanation of the applied technical principles. Those skilled in the art should understand that the scope of the invention involved in the present application is not limited to the technical solution formed by the specific combination of the above technical features, but should also cover other technical solutions formed by any combination of the above technical features or their equivalent features without departing from the concept of the present application. For example, the technical solution formed by mutually replacing the above features with the technical features (but not limited to) disclosed in the present application that have similar functions.

Claims

1. A humanoid robot motion coordination method based on joint torque, characterized in that, The method includes: Obtaining the original joint torque data of the humanoid robot through sensors, preprocessing the original torque data, and outputting stable torque data; Obtaining the joint angle data of the robotic arm of the humanoid robot through sensors, constructing an impedance control model by combining the robotic arm joint angle data with the stable torque data, and predicting the torque change trend to obtain a predicted torque sequence; Performing adaptive threshold detection on the predicted torque sequence to identify torque peaks of different amplitudes; Analyzing the propagation speed of the torque peak using a non-uniform sliding window method, and calculating the velocity vector of the peak propagation by combining the direction of the torque peak propagation; Judging whether the velocity vector of the peak propagation exceeds a preset threshold. If it exceeds, then combining the magnitude and direction of the peak propagation speed, and the peak amplitude change rate, predicting the response trend of the joint torque controller to judge whether it is necessary to adjust the joint control parameters of the humanoid robot; If it is necessary to adjust the joint control parameters of the humanoid robot, then optimizing the stiffness and damping parameters of the impedance control model according to the torque peak propagation speed vector and the amplitude change rate to obtain the optimized control parameters; For the optimized control parameters, calculating the amplitude of the controller output response signal. If the amplitude of the controller output response signal is within the preset range, then outputting a stable control signal.

2. The method according to claim 1, wherein The obtaining the original joint torque data of the humanoid robot through sensors, preprocessing the original torque data, and outputting stable torque data includes: Collecting the joint point data of the humanoid robot through an angular velocity sensor and a torque sensor to obtain the original torque data and the original angular velocity data; Calculating the angular velocity standard deviation of each joint point for the original angular velocity data, and obtaining the torque data corresponding to the stable time period according to the comparison result between the angular velocity standard deviation and the preset threshold; Performing decomposition processing on the torque data corresponding to the stable time period, screening each layer coefficient according to a preset energy threshold, and then performing signal reconstruction to obtain denoised torque data; Calculating the torque change rate between adjacent sampling points for the denoised torque data. If the torque change rate exceeds the preset torque change rate threshold, then correcting the abnormal points, and outputting stable torque data after correction.

3. The method according to claim 1, characterized in that, The obtaining the joint angle data of the robotic arm of the humanoid robot through sensors, constructing an impedance control model by combining the robotic arm joint angle data with the stable torque data, and predicting the torque change trend to obtain a predicted torque sequence includes: Obtaining the robotic arm joint angle data, and constructing an impedance control parameter matrix according to the stable torque data and the robotic arm joint angle data; Using the unit step response method to calculate the steady-state error and overshoot corresponding to the impedance control parameter matrix, and performing parameter correction on the calculated steady-state error and overshoot according to a preset threshold to obtain the corrected impedance control parameter matrix; Establishing a robotic arm motion state space equation according to the corrected impedance control parameter matrix; Calculating the state estimation value according to the robotic arm motion state space equation, calculating the state prediction value at a future moment according to the state estimation value and a preset prediction step length, and extracting the torque component from the state prediction value to obtain a predicted torque sequence.

4. The method according to claim 1, wherein Performing adaptive threshold detection on the predicted torque sequence to identify torque peaks of different amplitudes, including: Using a sliding time window to calculate the torque change rate of the predicted torque sequence, and normalizing the torque change rate sequence according to the maximum and minimum value method to obtain a normalized torque change feature sequence; Calculating the local mean and standard deviation based on the normalized torque change feature sequence to generate upper and lower dynamic threshold boundary curves; Marking the data points exceeding the boundary for the upper and lower dynamic threshold boundary curves, and using a time window to segment the marked points to obtain candidate torque peak intervals; Calculating the extreme points for the candidate torque peak intervals, judging the validity of the wave peaks according to the amplitude differences of the extreme points, and classifying the valid torque peaks using a clustering method to obtain torque peaks of different amplitudes.

5. The method according to claim 1, characterized in that, Analyzing the propagation speed of torque peaks using a non-uniform sliding window method, and calculating the velocity vector of peak propagation in combination with the direction of torque peak propagation, including: For torque peaks of different amplitudes, normalizing the peak amplitude according to the maximum peak amplitude, mapping the normalized amplitude to a preset weight interval through a linear function, and performing weighted combination on the normalized peak and a preset weight coefficient to obtain a weighted torque peak sequence; For the weighted torque peak sequence, establishing a three-dimensional coordinate system with the base of the robotic arm as the origin, using a geometric transformation method to obtain the position coordinates of the sensors in the three-dimensional coordinate system, and calculating the relative spatial position vectors between the sensors; Performing time window pairing on the peak points of adjacent sensors according to the position coordinates, and obtaining the propagation delay through the time information of the paired peak points; Fitting a propagation trajectory curve in combination with the propagation delay and the relative spatial position vectors of the sensors, extracting the unit tangent vector at each node of the propagation trajectory curve as the propagation direction, and combining the propagation speed magnitude and the propagation direction to obtain the velocity vector of propagation.

6. The method according to claim 1, wherein Judging whether the velocity vector of peak propagation exceeds a preset threshold. If it exceeds, then combining the magnitude and direction of the peak propagation speed, as well as the peak amplitude change rate, predicting the response trend of the joint torque controller to judge whether it is necessary to adjust the control parameters of the humanoid robot joints, including: Judging the overrun flag of the velocity vector of propagation according to the comparison result between the direction component of the peak propagation velocity vector and the reference velocity threshold; Processing the torque peak sequence using a sliding window gradient calculation method, and obtaining the joint controller response prediction parameters according to the overrun flag and the change rate of the torque peak sequence; Establishing a feedforward compensation matrix through the joint controller response prediction parameters, performing polynomial fitting on the controller output response curve, and calculating the deviation between the predicted curve and the controller output limit value; Dividing the controller saturation risk level interval for the deviation. If the controller is in the high-risk interval, then judging whether it is necessary to adjust the control parameters of the humanoid robot joints.

7. The method according to claim 1, wherein If it is necessary to adjust the control parameters of the humanoid robot joints, then optimizing the stiffness and damping parameters of the impedance control model according to the torque peak propagation velocity vector and the amplitude change rate to obtain the optimized control parameters, including: Form neural network input data according to the peak torque propagation speed vector and the amplitude change rate, and the neural network input data is used to construct a neural network; Process the neural network input data using the batch normalization method to obtain normalized data, and calculate the parameter adjustment direction using the gradient descent method based on the normalized data to obtain iterative parameters; Perform clipping processing on the iterative parameters to obtain clipped parameters, and the clipped parameters are updated to the impedance control model after being smoothed by the exponential moving average method to obtain optimized control parameters.

8. The method according to claim 1, characterized in that, For the optimized control parameters, calculate the amplitude of the controller output response signal. If the amplitude of the controller output response signal is within the preset range, output a stable control signal, including: Construct the impedance controller state equation according to the optimized stiffness parameter and damping parameter, and perform numerical solution using the Runge-Kutta method to obtain the response time-domain sampling sequence; Perform calculation on the response time-domain sampling sequence using the fast Fourier transform to obtain the response signal amplitude spectrum; Perform window sliding calculation on the response signal amplitude spectrum to obtain the normality test result of the amplitude distribution, and the normality test result is used to determine the correlation coefficient sequence; Design a Butterworth low-pass filter for bandwidth limitation for the correlation coefficient sequence, and perform numerical integration on the signal after bandwidth limitation to obtain the control output signal.

9. The method according to claim 1, characterized in that, The method further includes: obtaining the structural parameters of the robotic arm, calculating the error influence of elastic deformation on the original torque data at the joints of the humanoid robot using the finite element analysis method to obtain an error compensation coefficient; correcting the stable control signal through the error compensation coefficient to obtain the final control signal.

10. A humanoid robot motion coordination system based on joint torque, characterized in that, The system includes: A torque data processing module, which is used to obtain the original joint torque data of the humanoid robot through a sensor, preprocess the original torque data, and output stable torque data; An impedance control modeling module, which is used to obtain the robotic arm joint angle data of the humanoid robot through a sensor, construct an impedance control model by combining the robotic arm joint angle data and the stable torque data, and predict the torque change trend to obtain a predicted torque sequence; A peak detection module, which is used to perform adaptive threshold detection on the predicted torque sequence to identify torque peaks of different amplitudes; A propagation speed analysis module, which is used to analyze the propagation speed of torque peaks using the non-uniform sliding window method, and calculate the speed vector of peak propagation in combination with the direction of torque peak propagation; A response trend prediction module, which is used to determine whether the speed vector of peak propagation exceeds a preset threshold. If it exceeds, predict the response trend of the joint torque controller in combination with the magnitude and direction of the peak propagation speed, and the peak amplitude change rate, to determine whether it is necessary to adjust the control parameters of the humanoid robot joints A parameter optimization module, which is used to optimize the stiffness and damping parameters of the impedance control model according to the torque peak propagation speed vector and the amplitude change rate if it is necessary to adjust the control parameters of the humanoid robot joints, to obtain optimized control parameters; An output stability judgment module, which is used to calculate the amplitude of the controller output response signal for the optimized control parameters. If the amplitude of the controller output response signal is within the preset range, output a stable control signal; An error compensation module is used to obtain the structural parameters of the robotic arm, calculate the error influence of elastic deformation on the original torque data at the joints of the humanoid robot by using the finite element analysis method, and obtain the error compensation coefficient; the stable control signal is corrected by the error compensation coefficient to obtain the final control signal.

Images